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Abstract 

An  advance  in  the  development  of  smart  munitions  entails  autonomously  modifying 
target  selection  during  flight  in  order  to  maximize  the  value  of  the  target  being  destroyed. 
Target  identification  and  classification  provides  a  basis  for  target  value  which  is  used  in 
conjunction  with  multi-target  tracks  to  determine  an  optimal  aimpoint  for  the  munition. 
A  unique  guidance  law  can  be  constructed  that  exploits  attribute  and  kinematic  data  from 
an  onboard  video  sensor.  This  thesis  develops  an  innovative  path  planning  algorithm  that 
provides  an  obstacle  avoidance  function  while  navigating  the  munition  toward  the  highest 
value  target.  The  foundation  of  this  path  planning  method  is  found  in  the  principles  of 
minimum  effort  control  optimization.  Results  demonstrate  the  ability  of  the  path  planning 
algorithm  to  determine  a  path  for  the  munition  to  follow  which  is  both  stable  and  feasible. 
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A  MINIMUM  EFFORT  CONTROL  APPROACH 


TO  GUIDED  MUNITION  PATH  PLANNING 

I.  Introduction 

Over  the  past  decade,  smart  munitions  have  played  an  increasing  role  in  modern 
conventional  warfare.  Due  to  the  impressive  success  of  these  guided  munitions,  recent 
efforts  have  focused  on  boosting  the  agility  and  efficiency  of  this  class  of  weapons.  One 
way  this  performance  boost  can  be  achieved  is  by  allowing  autonomous  in-flight  retargeting 
of  the  munition  such  that  its  flight  terminates  at  the  most  valuable  target  in  the  scenario. 
This  increased  capability  does  not,  however,  come  without  its  own  cost.  The  munition 
must  now  be  able  to  ensure  it  avoids  obstacles  that  appear  in  the  path  to  the  new  target.  In 
addition,  once  an  obstacle-free  path  is  determined,  a  method  must  be  realized  to  generate 
an  optimal  set  of  control  inputs  to  navigate  along  this  path. 

Despite  this  desire  for  greater  autonomy  and  capability,  current  guidance  algorithms 
typically  lack  the  ability  to  modify  target  selection  after  launch  or  to  avoid  obstacles 
identified  by  the  vehicle’s  onboard  sensors.  Standard  guidance  algorithms  simply  force 
the  vehicle  to  fly  along  the  unobstructed  line-of-sight  (LOS)  trajectory  between  the  vehicle 
and  its  target  while  maintaining  the  predetermined  target  assignment.  In  a  multiple-target 
scenario,  the  terminal  waypoint  may  shift  dramatically  when  the  target  tracker  determines 
that  a  new  target  has  the  highest  value.  Any  attempt  at  achieving  online  retargeting 
creates  a  mandate  for  the  guidance  algorithm  to  perform  an  obstacle  avoidance  function 
whenever  the  terminal  waypoint  moves.  Furthermore,  given  that  mobile  friendly  and 
neutral  objects  may  be  classified  as  obstacles,  this  avoidance  function  will  also  be  invoked 
as  these  objects  move  about  the  battlefield.  Thus,  the  need  to  formulate  a  guidance 
algorithm  capable  of  driving  the  vehicle  to  the  most  valuable  target  still  exists. 

The  basic  scenario  is  shown  in  Figure  1.1.  Initially,  since  the  line-of-sight  path 
between  the  munition  and  the  target  is  blocked  by  a  mountain,  a  path  is  planned  to  guide 
the  munition  around  the  obstacle  and  impact  the  target.  This  planned  guidance  path  is 
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Figure  1.1  Notional  scenario  with  contour  map  of  a  mountain  -  1  target  in  view,  1 
obstacle  in  LOS 

illustrated  by  the  dotted  line  between  the  munition  and  the  troop  transport.  However, 
once  the  munition  flies  around  the  side  of  the  mountain  and  a  new  target  is  detected, 
the  entire  scene  must  be  reevaluated  from  the  viewpoint  of  the  munition.  As  a  result  of 
this  evaluation,  one  of  the  potential  targets  is  declared  to  have  the  highest  value  and  the 
munition  must  determine  a  new  path  in  order  to  engage  the  newly  designated  target.  A 
depiction  of  this  endgame  scenario  is  shown  in  Figure  1.2,  where  the  solid  red  line  indicates 
the  path  that  has  already  been  flown  and  the  munition  has  determined  that  the  value  of 
the  tank  is  greater  than  that  of  the  troop  transport. 

Some  researchers  have  explored  nonlinear  model  predictive  control  (MPC)  as  the 
basis  for  a  method  to  attain  this  obstacle  avoidance  capability  [9, 15, 16].  The  crux  of 
this  approach  lies  in  placing  limits  on  the  extent  to  which  the  vehicle  state  (typically 
position,  velocity,  acceleration,  and  other  navigation-related  states)  and  control  input  are 
allowed  to  deviate  from  their  nominal  values.  A  constrained  optimization  problem  is 
then  solved  in  order  to  generate  an  input  that  will  fly  the  vehicle  along  the  optimal  path 
between  a  set  of  known  waypoints.  In  general,  efforts  involving  this  approach  have  been 
limited  to  a  2-dimensional  world.  When  a  third  dimension  is  even  considered,  the  problem 
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Figure  1.2  Notional  endgame  with  contour  map  of  a  mountain  -  1  new  target  found, 
target  designation  updated 
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is  reduced  back  to  a  2-dimensional  setting  in  which  it  is  assumed  that  the  vehicle  will 
simply  travel  either  over  or  around  an  obstacle,  thereby  eliminating  the  opportunity  for 
cooperative  motion  between  the  horizontal  and  vertical  planes  of  the  vehicle.  For  example, 
if  an  air  vehicle  attempts  to  fly  over  an  obstacle,  this  MPC  approach  will  maintain  the 
current  heading  while  commanding  changes  to  altitude.  Although  the  desire  to  extend 
this  approach  to  a  full  3-dimensional  world  is  apparent,  the  specific  manner  to  accomplish 
this  evolution  is  not  as  obvious. 

Other  researchers  have  made  efforts  to  utilize  a  circle-line-circle  (CLC)  approach  to 
navigating  a  set  of  waypoints  [1-3,5,8,20].  Essentially,  this  method  constrains  the  vehicle 
to  fly  along  the  LOS  between  successive  waypoints  and  attempts  to  compute  the  time  to 
begin  the  turn  to  the  next  straight-line  segment  based  on  the  assumption  that  the  air 
vehicle  will  execute  all  heading  changes  at  a  constant  turn  rate.  The  flexibility  of  this 
approach  is  realized  by  its  ability  to  determine  a  minimum-time  path,  a  path  which  passes 
directly  through  the  given  waypoint,  or  any  path  in  between  these  two  which  satisfies 
the  assumed  system  constraints.  Current  efforts  in  this  arena  have  only  considered  2- 
dimensional  problems  (typically  the  horizontal  plane).  The  extension  of  this  approach 
to  a  full  3-dimensional  environment  presents  some  interesting  issues,  such  as  the  need  for 
additional  system  constraints. 

A  third,  conceptually  simple,  method  employs  the  principles  of  minimum  effort  con¬ 
trol  to  achieve  the  obstacle  avoidance  function.  When  presented  with  a  set  of  potential 
waypoints,  this  control  optimization  technique  can  readily  determine  the  path  which  re¬ 
quires  the  least  amount  of  applied  control  energy. 

All  three  of  these  control  optimization  methods  require  a  set  of  desired  waypoints  as 
an  input  to  the  procedure.  Bearing  this  requisite  input  in  mind,  there  remains  a  need  for 
a  manner  to  determine  these  waypoints,  especially  when  the  vehicle  operates  in  a  changing 
environment. 

1.1  Research  Goals  and  Contributions 

Motivated  by  the  desire  to  fly  an  air  vehicle  toward  the  highest  value  target  within 
a  scene,  this  research  attempts  to  generate  a  3-dimensional  guidance  algorithm  capable  of 
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navigating  the  vehicle  through  a  set  of  waypoints  such  that  obstacles  are  avoided  in  an 
environment  in  which  the  designated  target  is  prone  to  change.  Specifically,  a  multiple- 
target  tracker  will  be  created  to  provide  the  munition  with  information  regarding  the 
position,  velocity,  and  value  of  the  various  targets  within  the  scenario.  Additionally,  a 
path  planning  method  will  be  developed  to  formulate  a  flight  path  that  facilitates  obstacle 
avoidance.  These  two  pieces  (multi-target  tracker  and  path  planner)  comprise  the  entire 
guidance  algorithm.  Finally,  the  three  control  optimization  techniques  (model  predictive 
control,  circle-line-circle,  and  minimum  effort  control)  will  each  be  utilized  as  the  basis  of 
a  method  to  determine  the  optimal  set  of  control  inputs  required  to  have  the  air  vehicle 
traverse  this  reference  trajectory. 

The  primary  contribution  of  this  research  is  the  utilization  of  the  minimum  effort 
control  optimization  technique  as  the  basis  for  a  path  planning  algorithm.  Although 
minimum  effort  control  has  been  utilized  for  generating  optimal  control  inputs,  the  notion 
of  using  this  control  optimization  approach  as  the  foundation  for  a  path  planner  has  not 
been  attempted  by  other  researchers  in  the  navigation  field.  Secondary  contributions  of 
this  research  are  the  attempts  to  extend  CLC  and  MPC  control  optimization  techniques 
to  3-dimensional  situations. 

1.2  Scope  and  Assumptions 

First  and  foremost,  although  the  ultimate  intention  is  to  utilize  a  procedure  of  this 
nature  in  online  operation,  the  real-time  operation  constraint  will  not  be  imposed  upon 
this  guidance  algorithm.  This  relaxed  approach  is  taken  since  this  research  still  falls  within 
the  realm  of  concept  exploration.  Given  that  this  research  is  only  investigating  the  con¬ 
cept’s  general  feasibility  and  is  not,  at  present,  designated  for  application  on  any  specific 
airframe,  the  tests  will  be  conducted  using  a  six-degree-of-freedom  F-16  flight  dynamics 
model  (already  available  in-house)  in  place  of  any  specific  munition  model.  Additionally, 
it  will  be  assumed  that  autopilots  capable  of  maintaining  commanded  velocity,  heading 
and  altitude  settings  are  available  for  use  with  the  specified  air  vehicle  model.  Further¬ 
more,  in  an  effort  to  reduce  the  complexity  of  the  problem,  it  will  be  assumed  that  we 
desire  to  maintain  a  constant  velocity  during  each  of  the  simulations.  Finally,  for  ease 
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of  implementation  in  this  early  stage  of  concept  exploration,  it  will  be  assumed  that  the 
munition  has  knowledge  of  all  the  obstacles  within  the  scene,  including  those  that  would 
normally  be  occluded  from  view. 

1.3  Thesis  Organization 

Chapter  2  presents  the  theoretical  background  information  required  to  understand 
the  development  and  application  of  this  guidance  algorithm.  Section  2.2  provides  details 
on  target  tracking  concepts,  to  include  stochastic  estimation  and  linear  Kalman  filtering 
(Section  2.2.1)  and  types  of  target  models  (Section  2.2.2).  Section  2.3  discusses  various 
methods  of  control  input  generation.  Section  2.3.1  presents  the  concept  of  circle-line-circle 
control,  while  Section  2.3.2  details  the  theory  of  the  model  predictive  control  method.  The 
basic  properties  of  controllability  matrices  and  Grammians  are  presented  in  Section  2.3.3 
and  their  applicability  to  the  theory  behind  minimum  effort  control  is  shown  in  Section 
2.3.4.  Section  2.4  presents  the  basic  simulation  model  used  throughout  this  research. 

Chapter  3  describes  the  development  of  the  guidance  algorithm  and  provides  details 
regarding  the  simulation.  Section  3.2  details  how  the  standard  multi-target  tracker  is 
modified  to  provide  the  unique  information  required  by  this  guidance  algorithm.  Section 

3.3  then  discusses  the  method  implemented  to  generate  waypoint  sets  for  the  purpose  of 
obstacle  avoidance.  Section  3.4  describes  the  application  of  the  minimum  effort  control 
technique  in  solving  the  problem  of  selecting  the  optimal  path  from  the  candidate  waypoint 
sets.  These  three  sections  provide  a  concise  description  of  the  development  of  the  entire 
guidance  algorithm.  Section  3.5  presents  the  extension  of  the  MPC  and  CLC  control 
schemes  to  three  dimensions  and  then  demonstrates  how  minimum  effort  control  is  used  to 
generate  the  commands,  to  be  sent  to  the  vehicle’s  three  autopilots,  based  on  the  waypoint 
information  contained  within  the  selected  path.  Finally,  Section  3.6  details  the  adaptation 
of  the  simulation  model  to  accommodate  the  various  innovations  of  this  research. 

Chapter  4  presents  an  analysis  of  simulation  results.  The  guidance  path  selected  by 
the  path  planning  algorithm  developed  in  this  thesis  will  be  compared  with  the  minimum 
distance  trajectory  for  that  particular  test  case  in  order  to  illustrate  the  key  benefits  and 
shortcomings  of  each  trajectory  generation  method. 
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Chapter  5  concludes  this  research  by  summarizing  the  algorithm  development  of 


Chapter  3  and  the  results  obtained  in  Chapter  4.  Furthermore,  it  presents  recommenda¬ 
tions  for  future  research  regarding  this  concept. 
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II.  Background 


2.1  Introduction 

The  desire  to  guide  an  air  vehicle  toward  a  moving  target  immediately  invokes  the 
need  to  have  a  method  to  determine  the  target’s  state  information  (position,  velocity,  ID, 
etc.).  Section  2.2  is  devoted  to  exploring  the  basics  of  target  tracking.  Given  that  most 
modern  target  trackers  rely  on  Kalman  filtering  to  provide  estimates  of  the  target’s  state  [6] , 
the  details  of  stochastic  state  estimation  in  relation  to  Kalman  filters,  and  the  details  of 
several  classes  of  target  models  (to  be  used  in  the  Kalman  filter)  are  presented  in  this 
section.  Section  2.3  addresses  the  fundamental  concepts  of  the  three  control  generation 
methods  that  will  be  investigated  by  this  research.  A  subsection  on  controllability  matrices 
and  Grammians  is  included  to  assist  the  discussion  of  minimum  effort  control.  Section 
2.4  presents  the  basic  F-16  system  model  which  provides  the  environment  for  conducting 
simulations. 

2.2  Target  Tracking  Basics 

As  mentioned  in  the  introduction,  the  Kalman  filter  is  perhaps  the  most  widely  used 
estimation  technique  in  current  target  tracking  algorithms.  Though  they  are  not  generally 
thought  of  in  this  fashion,  for  the  purposes  of  target  tracker  design,  Kalman  filters  may  be 
split  into  two  pieces.  The  first  part  consists  of  the  development  of  the  equations  which 
define  the  linear  Kalman  filter.  This  piece  is  well  grounded  in  the  theory  of  stochastic 
estimation  and  the  form  of  the  equations  generally  remains  the  same  from  filter  to  filter. 
The  other  portion  of  the  Kalman  filter  is  the  user-defined  component  and  primarily  entails 
the  selection  of  a  model  to  describe  the  motion  of  the  expected  target  type.  Bearing  this 
all  in  mind,  we  now  turn  to  the  discussion  of  these  two  aspects  of  target  tracking. 

2.2.1  Stochastic  Estimation  and  Linear  Kalman  Filtering.  The  development  of 
the  linear  Kalman  filter  equations  in  this  section  is  an  adaptation  of  the  material  presented 
in  the  Maybeck  text  [12] .  This  material  is  intended  to  be  a  summary  of  the  key  points  of 
linear  Kalman  filtering  and  its  stochastic  estimation  underpinnings.  The  interested  reader 
is  directed  to  [12]  for  a  more  in-depth  derivation  of  the  filter  equations. 
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Let  us  assume  that  we  wish  to  estimate  the  state  values  for  a  linear  system  which 
can  be  described  by  the  following  discrete-time  state-space  model  (this  may  also  be  an 
equivalent  discrete-time  model  that  was  derived  from  an  underlying  continuous-time  model 
description,  as  shown  in  Section  2.4  of  [12]): 


x(ti)  =  +  Gd(ti-i)wd(ti-i) 

z(ti)  =  H(ti)x(ti)  +  v(ti)  (2.1) 

where  x{U)  is  the  system  state  vector  and  z(U)  is  a  noise-corrupted  measurement  vector 
of  the  (potentially  time-varying)  system  state  at  time  sample  tj.  <3>(tj,ij_i)  is  the  state 
transition  matrix  which  describes  the  homogeneous  motion  of  the  system  state  from  time 
sample  f,_i  to  L,  Gd(U- 1)  is  a  noise  weighting  matrix  based  on  the  system  model  de¬ 
scription,  and  H(ti )  is  the  measurement  matrix  which  defines  the  specific  combinations  of 
states  that  are  represented  by  the  measurements.  The  wd(U-i)  and  v(U)  terms  represent 
mutually  independent  discrete-time  white  Gaussian  noise  vectors  with  the  following  mean 
and  variance  statistics: 


E{wd(U)}  =  E{v(ti)}  =  0 

E{wd(ti)wd{tj)T}  =  Qd{ti)5ij 

E{v(ti)v(tj)T }  =  R(ti)5ij  (2.2) 

where  Qd(U )  is  the  process  noise  covariance  matrix,  R(U)  is  the  measurement  noise  covari¬ 
ance  matrix,  and  8ij  is  the  well  known  Kronecker  delta  function.  In  these  expressions,  the 
subscript  d  indicates  that  this  is,  in  fact,  either  a  discrete-time  model  or  the  discrete-time 
equivalent  of  a  continuous-time  model.  The  process  and  measurement  noises  are  also 
assumed  to  be  independent  of  the  initial  system  state  vector,  x(to).  In  looking  forward  to 
the  goal  of  designing  a  target  tracking  filter,  Qd(U)  can  be  thought  of  as  a  measure  of  our 
confidence  in  the  assumed  target  dynamics  model  while  R(ti)  represents  the  quality  of  our 
measuring  device  (sensor). 
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Furthermore,  let  us  also  assume  that  the  initial  state,  x(to),  is  well  described  by  a 
Gaussian  probability  density  function  (PDF)  with  mean  rc(to)  and  covariance  matrix  P(to). 
It  can  then  be  shown  that  the  conditional  density  function  fx(ti)\z(ti)(€\Zi),  f°r  the  state 
x{ti)  conditioned  on  the  measurement  history  up  until  that  time  (Z(ti)),  remains  Gaussian 
with  conditional  mean  )  and  conditional  covariance  Pftf ).  One  important  distinction 
between  Z{tf)  and  z(U)  must  be  made  here;  Z{tf)  is  the  history  of  all  measurements  from 
time  to  to  time  tt.  while  z(tt)  is  merely  the  measurement  at  time  fj.  It  can  also  be  proven 
that  the  conditional  density  function  fx(ti)\z(ti-1)(£,\Zi-i),  for  x(U)  conditioned  on  the 
previous  measurement  history  (Zj_ i),  maintains  its  Gaussian  structure  with  conditional 
mean  x{t~)  and  conditional  covariance  P(t f)  [12].  The  conditional  density  function  for 
the  propagated  state  is  recognized  as  fxfti^zfti-^UlZi-i),  while  <4(ti)|z(ti)(£|^i)  is  the 
conditional  density  function  for  the  updated  state.  This  distinction  is  further  emphasized 
by  the  superscript  notation  in  which  "  — "  indicates  first  and  second  order  statistics  before  a 
measurement  update,  whereas  "+"  indicates  these  same  statistics  after  the  incorporation 
of  the  current  measurement.  The  conditional  mean  and  covariance  for  both  the  propagated 
and  the  updated  states  are  defined  as: 


) 

=  E{x(ti)\Z(U-i)  =  Zi- 1} 

ntf) 

=  E{[x(ti)  -x(t7)][x(U)  -  x(t7)]T\Z(ti-i)  =  Zj_i} 

mt) 

=  E{x(ti)\Z{ti)  =  Zi] 

P(tf) 

-  E{[x{u)  -  -  x(4)]T\z{ti)  =  z^ 

(2.3) 

When  we  talk  about  the  Kalman  filter’s  estimate  of  the  target  state  vector,  we  will  typically 
be  referring  to  the  conditional  mean  of  the  updated  state.  Given  that  the  expected  value  is 
a  linear  operation  and  that  the  state  transition  matrix  is  a  purely  deterministic  quantity,  we 
are  now  ready  to  present  the  explicit  expressions  for  the  conditional  mean  and  covariance. 
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For  the  propagated  state  estimate,  we  may  substitute  the  assumed  form  of  x(ti)  from 
Equation  (2.1)  into  the  definitions  of  x(E~)  and  P(t~ )  in  Equation  (2.3)  and  solve  to  obtain 

Wi)  =  l)I(tti) 

P{ti)  =  ${ti,ti-i)P(tf_  i)$T(tj,tj_i)  +  Gd(ti-i)Qd(ti-i)Gd(ti-i)  (2.4) 

In  the  case  of  the  updated  state  estimate,  we  first  note  that  x_(tf)  and  P(tf)  are  the 
mean  and  covariance  associated  with  the  conditional  PDF  fx(ti)\z(ti)(£\Zi),  which  may  be 
rewritten  as 

fz(ti)\x(ti),Z(ti-i){C_j\£i  l)fx(ti)\Z(ti-i){QZi—  i)  ^  ^ 

after  a  few  applications  of  Bayes’  rule.  It  can  then  be  shown  that,  for  the  case  of  a  linear 
measurement  model,  this  conditional  PDF  remains  Gaussian  with  mean  and  covariance 
given  by  [12] 


fx(ti)\Z(ti)  (£l  ^i) 


mi)  =  wti ) + K{ti)\zi  -  )] 

P(tf)  =  P(tr)-K(U)H(U)P(tr) 

K(U)  =  Pitr^itJiHitjPitr^M  +  RiU)}-1  (2.6) 

where  K(ti )  is  referred  to  as  the  Kalman  gain.  At  this  point,  it  should  be  noted  that, 
given  the  structure  of  the  equations,  the  entire  time  history  of  the  Kalman  gain  and  both 
the  propagated  and  updated  covariances  may  be  computed  from  the  system  definition  and 
the  initial  covariance  P(to)  without  requiring  access  to  the  actual  real-time  measurements. 

2.2.2  Target  Models.  Now  that  we  have  the  structure  of  the  linear  Kalman  filter, 
we  are  ready  to  investigate  the  other  essential  part  of  this  target  tracking  filter:  the  target 
dynamics  model.  A  plethora  of  dynamics  models  exist,  all  based  on  different  assumptions 
about  the  behavior  of  the  target  system.  While  it  might  be  interesting  to  investigate 
the  full  realm  of  target  models,  our  intent  is  only  to  present  the  details  of  a  few  of  the 
more  commonly  used  model  types.  To  that  end,  we  will  present  the  state  transition  and 
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Figure  2.1  Block  diagram  of  FOGMA  model 


process  noise  matrices  for  the  first-order  Gauss-Markov  acceleration  (FOGMA)  model,  the 
constant  velocity  (CV)  model,  and  the  coordinated  turn  model. 


2.2.2. 1  First-Order  Gauss-Markov  Acceleration  Model.  As  the  name  im¬ 
plies,  the  FOGMA  model  assumes  that  the  acceleration  of  the  target  is  adequately  de¬ 
scribed  by  a  first-order  lag  system  driven  by  white  Gaussian  noise  (of  appropriate  strength) . 
In  general,  this  model  also  assumes  that  the  dimensions  of  the  system  (x,  y,  and  z  directions 
for  instance)  may  be  decoupled,  such  that  if  one  were  to  develop  the  state  transition  (3>) 
and  process  noise  (Qd)  matrices  for  position,  velocity,  and  acceleration  in  one  direction, 
then  the  full  dimension  system  would  be  described  by  the  use  of  independent  copies  of  the 
and  Qd  matrices. 

This  model  is  shown,  conceptually,  in  Figure  2.1  for  the  continuous-time  case.  Based 
on  this  model,  the  continuous-time  state  equations  have  the  form 


II 

•  H 

vx(t ) 

II 

.  H 

ax  (t) 

ax{t)  = 

1 

T  m 

ax(t)  +  wx(t) 


(2.7) 
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where  rm  is  the  time  constant  of  the  target’s  maneuver  and  wx(t )  is  continuous-time  zero- 
mean  white  Gaussian  noise  of  variance  (this  is  also  the  mean-squared  acceleration). 
Converting  this  to  a  discrete-time  representation,  we  obtain  the  following  state  transition 
matrix  [6,10] 


1  T 


+  TZ  +  e 


—  T/t  n 


4>  = 


0  1 
0  0 


(2.8) 


where  T  is  the  sample  interval  for  the  discrete-time  system  and  the  exact  form  of  the 
process  noise  matrix  may  be  found  in  Section  4.2.1  of  [6].  Assuming  that  the  sample 
interval  (T)  is  significantly  less  than  the  maneuver  time  constant  (rm),  we  obtain  the 
simplified  state  transition  and  process  noise  matrices  [4, 6, 10] 
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Note  that  this  limiting  case  of  the  FOGMA  model  is  actually  the  definition  of  the  constant 
acceleration  model,  in  which  jerk  (the  time  derivative  of  acceleration)  is  modeled  as  zero- 
mean  white  Gaussian  noise. 

The  assumption  that  T  <C  rm  is  not  a  requisite  characteristic  of  the  FOGMA  model; 
however,  it  permits  the  use  of  significantly  simplified  definitions  for  the  and  Qd  matrices. 
In  addition,  as  T  approaches  rm,  the  accuracy  of  our  acceleration  estimate  continually 
degrades  until,  for  T  2>  rm,  we  essentially  lose  the  ability  to  track  the  target’s  acceleration. 
At  this  point,  we  would  be  better  off  switching  to  a  constant  velocity  model  since  the 
acceleration  appears  white  instead  of  time-correlated. 


2. 2. 2. 2  Constant  Velocity  Model.  The  CV  model  is  based  on  the  assumption 
that  the  target’s  acceleration  is  adequately  described  by  zero-mean,  white  Gaussian  noise 
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Figure  2.2  Block  diagram  of  CV  model 

and  the  velocity  essentially  remains  constant.  As  shown  in  Figure  2.2,  the  model  for 
the  velocity  state  is  depicted  as  an  integrator  driven  by  zero-mean  white  Gaussian  noise. 
Similar  to  the  FOGMA  model,  the  CV  model  also  assumes  a  decoupling  of  the  target 
motion  in  the  x,  y,  and  z  directions. 

Based  on  the  model  in  Figure  2.2,  the  continuous-time  state  equations  are 

x(t)  =  vx(t) 

vx(t)  =  0  +  wx(t)  (2-10) 


The  state  transition  and  process  noise  matrices  are  then  [4, 6] 


<4* 


Qd,CV 


1  T 
0  1 

j^2 


(2.11) 


where  q  is  the  strength  of  the  zero- mean  white  Gaussian  noise  used  to  define  the  target’s 
acceleration.  As  implied  in  our  discussion  of  the  FOGMA  model,  the  CV  model  is  seen  to 
be  a  limiting  case  of  the  FOGMA  model  for  the  situation  where  rm  is  significantly  smaller 
than  T. 


2. 2. 2. 3  Coordinated  Turn  Models.  The  two  target  models  discussed  so  far 
have  assumed  that  is  it  possible  to  decouple  a  3-dimensional  problem  (x,  y,  z  directions) 
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into  three  completely  separate  1-dimensional  problems.  This  assumption  is  not  always 
valid,  as  in  the  case  of  on  aircraft  performing  a  coordinated  turn  [6].  In  this  situation, 
target  motion  in  one  direction  will  provide  information  regarding  complementary  motion  in 
one  or  both  of  the  remaining  directions.  This  gives  rise  to  the  coordinated  turn  model  [6], 
wherein  it  is  assumed  that  the  turn  is  accomplished  at  a  nearly  constant  rate  of  turn  (w). 

For  a  coordinated  horizontal  (x,y)  turn,  where  we  assume  the  third  direction  of 
motion  acts  independent  of  the  other  two  directions,  the  state  transition  matrix  (corre¬ 
sponding  to  the  state  vector  [ x  vx  y  vy  u]T)  and  the  process  noise  matrix  for  the  coupled 
directions  are  given  by 
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(2.12) 


where  the  elements  of  <f>  are  evaluated  at  the  current  state  vector  estimate,  a is  the 
variance  of  the  random  acceleration,  and  cr^  is  the  turn  rate  variance.  The  elements  in 
the  final  column  of  the  state  transition  matrix  are  the  partial  derivatives  of  the  non-linear 
discrete-time  function  with  respect  to  the  turn  rate  and  are  given  by 


(ujT  cos (ujT)  —  sin(u;T))  vx  —  {ujT  sin(wT)  —  1  +  cos(ccT))  vy 
‘his  =  - 5 - 

$25  =  —  (T  sin(uT)vx  +  T  cos(uT)vy) 

( ojT  sin(caT)  —  1  +  cos(cjT))  vx  +  ( wT cos (wT)  —  sin(cuT))  vy 

$35  =  - 9 - 

ur 

$45  =  T  cos  (ujT)vx  —  T  sin  (ujT)vy  (2-13) 
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2.3  Control  Concepts 

In  this  section,  we  delve  into  a  few  different  control  optimization  techniques.  Given 
that  one  of  the  goals  of  this  research  is  to  evaluate  the  utility  of  the  CLC  and  MPC 
approaches  in  a  3-dimensional  setting,  we  will  first  present  the  background  of  the  work 
that  has  been  accomplished  using  these  control  methods  for  2-dimensional  problems.  Our 
brief  discussion  of  control  concepts  will  conclude  with  a  presentation  on  the  theory  of 
minimum  effort  control. 

2.3.1  Circle-Line-Circle  Control.  The  driving  concern  behind  the  CLC  control 
optimization  method  [1,2]  is  that  there  are  limits  imposed  on  the  magnitude  of  the  control 
inputs  and  there  is  a  desire  to  minimize  deviations  from  a  LOS  path  between  waypoints. 
When  a  non-zero  input  is  applied,  the  desire  is  to  have  the  magnitude  of  the  optimal  input 
be  equal  to  an  upper  bound.  By  employing  this  proposed  control  scheme,  a  vehicle  would 
traverse  a  set  of  waypoints  by  following  the  straight-line  path  between  successive  waypoints 
until  a  certain  time  (known  as  the  switching  time),  when  a  series  of  turns  are  executed  at  the 
vehicle’s  maximum  turn  rate,  in  order  to  reach  the  next  straight-line  segment.  A  notional 
view  of  the  trajectory  generated  by  this  manner  of  control  is  shown  in  Figure  2.3.  Note 
that  in  order  to  execute  a  sharp  turn  to  the  right  at  wp  ,,  one  would  first  command 
a  short  turn  to  the  left  in  an  effort  to  smooth  the  transition  into  the  rightward  turn.  A 
similar  smoothing  turn  would  be  executed  at  the  end  of  the  turn  in  order  to  transition  back 
to  the  straight-line  segment.  All  that  remains  is  a  need  to  determine  the  switching  time, 
and  then  one  would  have  an  optimal  control  method  that  incorporates  constraints  on  the 
control  input.  Currently,  this  method  has  only  been  applied  to  2-dimensional  problems. 

Since  the  theory  of  this  control  mechanism  relies  heavily  on  Pontryagin’s  Minimum 
Principle,  we  shall  start  off  by  outlining  the  details  of  this  principle  [1, 19].  Let  us  adopt 
the  form  of  the  general  nonlinear  dynamic  system  given  by 

x(t)  =  f(x(t),u(t))  (2.14) 
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WPcurrent 


Figure  2.3  CLC  conceptual  trajectory  for  transition  between  straight-line  segments  de¬ 
fined  by  the  waypoints  wp previous,  wp current,  and  wpnext 

with  initial  condition  x{to),  final  state  constraint  set 


X  =  {x(tf)  G  Mna:|0(x)  =  0} 


(2.15) 


and  input  constraint  set 


li  =  {ue  Mn“|H(u)  >  0} 


(2.16) 


for  the  vectors  of  continuously  differentiable  functions  defined  by  /,  0  and  5.  For  the 
functions  in  the  constraint  sets,  let  us  assume  that  the  gradient  matrices,  and  , 

consist  of  linearly  independent  gradient  vectors  for  all  possible  x  and  u,  respectively.  Now, 
if  u*  is  an  admissible  and  optimal  control  input,  optimal  in  the  sense  that  it  minimizes  the 
cost  function 

J  =  f  dtC (x(t) ,  u(t))  (2-17) 

Jo 

then  a  continuous  piecewise-differentiable  function  y(t)  =  [71  •  •  •^nx]T ,  a  constant  y0  >  0, 
and  a  constant  vector  p  =  [pl  ■  ■  ■  png]T  are  guaranteed  to  exist  and  satisfy  the  following 
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conditions  [1]: 
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7o 

+ 

0 

(2.18d) 

7 

0 

(2.18e) 

where  H  is  the  system  Hamiltonian  given  by 

H(x{t),u{t),y{t),i0)  =  j0C(x(t),u(t))  +yr(t)f(x(t),u(t))  (2.19) 

Furthermore,  the  Hamiltonian  takes  on  a  global  minimum  almost  everywhere  when  evalu¬ 
ated  along  the  trajectory  resulting  from  the  application  of  the  optimal  input: 

H(x(t),u*(x{t)),'y(t),'y0)  =  min(i7(x(t),u(t),7(t),70))  =  0  (2.20) 

—  u£fi  — 

Note  that  Equation  (2.18a)  is  a  backward  time  propagation  for  the  adjoint  state,  7 (t),  and 
Equation  (2.18c)  is  the  terminal  condition  for  this  propagation.  Caution  must  be  exercised 
when  applying  the  minimum  principle  in  that  the  conditions  in  Equation  (2.18)  are  only 
necessary  conditions.  No  guarantee  is  made  that  any  input  which  satisfies  these  four 
conditions  is  in  fact  an  optimal  control.  However,  all  inputs  which  satisfy  the  conditions 
of  Equation  (2.18)  and  Equation  (2.20)  are  referred  to  as  extremal  controls  and,  collectively, 
these  extremal  controls  comprise  the  set  of  possible  optimal  controls  for  a  given  problem. 
With  the  foundation  of  the  minimum  principle  of  Pontryagin,  the  research  in  [1]  proved  that 
commanding  an  input  that  has  magnitude  which  is  equal  to  the  upper  limit  of  the  control 
constraint  produced  an  extremal  input  for  the  class  of  problems  under  consideration. 

We  will  now  summarize  this  development  for  the  2-dimensional  navigation  problem. 
The  problem  begins  with  a  simple  set  of  kinematic  equations  for  the  motion  of  a  vehicle 
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in  a  horizontal  plane 


x(t)  =  scos(\l/(i)) 
y{t)  =  ssm(^(t)) 
®(t)  =  u(t) 


(2.21) 


where  x  and  y  are  position  coordinates,  s  is  a  constant  speed  and  T  is  the  vehicle  heading 
angle.  The  magnitude  of  the  input  is  constrained,  such  that 

ll^ll  7  umax  (2.22) 


to  account  for  the  fact  that  the  vehicle  is  not  capable  of  making  arbitrarily  fast  turns. 
Next,  it  is  desired  to  navigate  a  set  of  known  waypoints  in  a  time-optimal  fashion  while 
obeying  the  input  constraint.  Since  we  are  considering  a  time  minimization  as  our  criterion 
of  optimality,  the  appropriate  cost  function  to  be  minimized  is  simply 


J  = 


(2.23) 


From  this  point,  the  proof  of  an  extremal  control  becomes  the  evaluation  of  the  inequality 
implied  by  Equation  (2.20) 


H(x,  u*,  7, 70) 


H(x,u,  7, 70) 


<  H(x,u,  7,70),  \/u£y 

=  7o  +  7i(*)scos(^)  +  72(t)ssin('0)  +  j3(t)u(t) 


(2.24) 


which  simplifies  to 


<  j3(t)u(t),  VuG/r 


Given  this  final  inequality,  the  proposed  optimal  input  can  be  written  as 


(2.25) 


u*(t)  =  -sign^^t^Umax 


(2.26) 
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and  can  be  shown  to  be  an  extremal  control  by  evaluating  the  conditions  in  Equation 
(2.18). 

An  additional  result  of  the  Anderson  research  [1]  was  the  development  of  a  convenient 
method  to  force  the  trajectory  to  pass  through  a  point,  p  ,  near  the  current  waypoint, 


wp 


current 


This  desired  point  is  constrained  to  lie  on  the  bisector  of  the  straight-line 


segments  defined  by  wp  ,  wp  ±.  and  wp  ±.  Furthermore,  the  range  of  pos- 

°  17  — —previous 1  — —current  — —next  7  ° 

sible  locations  for  p  is  bounded  by  wp  ,  and  a  predetermined  minimum  point, 
Pmin-  The  family  of  allowable  trajectories,  as  shown  in  Figure  2.4,  is  characterized  by  a  n 
parameter  such  that  the  point  where  the  trajectory  crosses  the  bisector  is  given  by 


P 

—cross 


=  (1  —  k)wV  x  +  KT) 

v  7  1  current  ' 


Hi  = 


wp  ±  —  p 

— —current  —cross 


sin(^C) 


- 1 


(2.27) 


where  Pclc  is  the  angle  between  consecutive  straight-line  segments  and  k  6  [0, 1].  The 
trajectories  shown  in  Figure  2.4  represent  the  set  of  possible  paths  to  fly  when  executing 
a  constrained  turn.  In  general,  we  would  prefer  to  apply  the  constrained  turn  trajectory 
obtained  by  selecting  k  =  1,  since  this  option  produces  a  time-optimal  solution.  The 
switching  time  computation  for  this  general  set  of  trajectories  may  be  found  in  [1]. 


2.3.2  Model  Predictive  Control.  The  MPC  approach  [15]  is  a  quadratic  cost  min¬ 
imization  technique  that  falls  under  the  larger  heading  of  receding  horizon  control  (RHC) 
methods  [13].  It  allows  us  to  compute  an  optimal  set  of  controls  (valid  for  a  finite  length 
of  time)  to  navigate  between  a  set  of  known  waypoints  while  explicitly  addressing  state 
and  input  constraints.  This  optimal  set  of  controls  is  realized  as  an  optimal  perturbation 
about  an  assumed  nominal  input.  Receding  horizon  control  methods  seek  to  determine 
the  optimal  control,  u*(t),  for  the  non-linear  system  of  Equation  (2.14)  by  minimizing  the 
following  cost  function  [13]  over  the  finite  length  interval  from  time  t  to  time  t  +  T: 


1  rt+T 

J(x,t;u)  =  -  /  [xT (t)T x(t)  +  uT (t)T u(T)]di 

2  Jt 


(2.28) 
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WPcurrent 
K  =  0 


Figure  2.4  CLC  trajectories  with  k  parameter 

Within  the  framework  of  RHC,  model  predictive  control  operates  by  first  obtaining 
a  perturbational  linearization  of  the  system  about  some  nominal  control  input,  unom(t), 
and  system  output,  y  (t) ,  trajectories  such  that 

u{t)  =  unom{t)  +  5u{t) 

y(t)  =  ynom{t)  +  5y(t)  (2.29) 

Next,  a  set  of  control  basis  functions  is  determined  so  that  the  perturbation  input  and 
output  terms  may  be  rewritten  as  linear  combinations  of  a  single  set  of  scale  factors  [9,15] 

5u  =  Ur] 

5y  =  Yr]  (2.30) 

where  the  matrix  U  describes  the  control  basis  functions,  rj  is  the  vector  of  scale  factors, 
and  the  matrix  Y  describes  the  relationship  between  the  output  and  the  control  basis 
functions.  Note  that  the  U  and  Y  matrices  are  sized  such  that  they  contain  the  required 
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information  for  each  sample  instant  on  the  interval  from  time  t  to  time  t+T.  For  example, 
in  the  case  of  a  single  input  described  by  five  basis  functions,  the  dimension  of  the  U  matrix 
which  covers  a  length- N  time  interval  is  N  x  5.  The  form  of  U  is  dictated  by  the  choice  of 
control  basis  functions,  and  dramatic  changes  will  be  observed  based  on  the  choice  of  the 
basis  functions.  For  instance,  the  basis  functions  may  be  as  simple  as  a  set  of  ramp  [15] 
or  tent  [9]  functions  or  as  complex  as  a  set  of  Laguerre  or  Legendre  polynomials  [16].  The 
matrix  Y  is  determined  by  driving  the  non-linear  system  with  the  input  basis  functions 
one  at  a  time. 

Once  we  have  obtained  the  U  and  Y  matrices,  MPC  seeks  to  minimize  the  following 
cost  function  (shown  for  a  notional  discrete-time  system) 

fc+JV-l 

Jk  =  fei+i  -  Li+i)Tr(yi+i  -ri+1)  Yufruil  (2.31) 

i=k 

where  N  is  the  length  of  the  finite  interval  horizon,  rt  is  a  reference  output  trajectory  at 
time  sample  i.  and  T  and  T  are  cost  weighting  matrices.  Based  upon  the  perturbational 
linearization  of  the  system,  we  may  rewrite  the  cost  function  as 

k+N-l 

Jk  ~  ^  ^y.nom,i+ 1  -*+l)  ^^Lnom,i+ 1  “  -*+ 1)  -nom,i^^nom,i  +  ;  1 

i=k 

+  SufTSui  +  2 (ynomi+1  -  ri+1)TT5y.+1  +  2ulomiT8ui\  (2.32) 

Noticing  that  the  first  two  terms  represent  an  incurred  cost  that  is  independent  of  the 
optimization  and  remembering  the  unique  definition  of  the  dimensions  of  the  U  and  Y 
matrices,  we  may  drop  the  terms  that  do  not  involve  perturbation  variables  and  simplify 
the  cost  function  to  be 

Jk  =  rjr{YTVY  +  UTTU)r,  +  2 [(^  -  r)TrY  +  ulomTU)R  (2.33) 

where  the  columns  of  r  are  the  individual  time  samples,  r,,  of  the  reference  trajectory 
that  spans  the  time  interval  of  interest,  and  similar  definitions  apply  to  y,nom  and  unom- 
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Note  that  the  summation  in  Equation  (2.32)  is  embedded  in  the  multiplications  of  the 
higher-dimensioned  vectors  in  Equation  (2.33). 

In  preparation  to  solve  the  constrained  optimization  problem,  we  must  first  trans¬ 
form  the  constraints  on  the  input  and  output  into  constraints  on  the  basis  function  scale 
factors,  since  the  optimization  problem  is  actually  considering  the  perturbational  inputs 
and  outputs,  as  defined  by  Equation  (2.30),  instead  of  the  full-scale  values.  This  is  a  very 
simple  process  since  the  original  constraints  are  given  as 


—lower 

< 

u 

< 

11 

—upper 

—lower 

< 

y 

< 

V 

—upper 

(2.34) 


We  may  use  Equations  (2.29)  and  (2.30)  to  reform  these  constraints  as  upper  limits  on  the 
basis  function  scale  factors 


u 

—upper  ILnom 

-u 

VI 

s- 

— nom  —lower 

Y 

v  —  V 

—upper  —nom 

-Y 

v  —  V, 

L—nom  — Lower  J 

(2.35) 


The  MPC  problem  is  now  in  a  format  that  can  be  solved  by  the  MATLAB^  quadratic 
programming  function,  quadprog. 

In  general,  research  on  the  use  of  MPC  techniques  for  flight  control  optimization  has 
been  limited  to  2-dimensional  cases,  for  which  it  is  assumed  that  an  autopilot  is  capable  of 
holding  the  third  dimension  of  motion  stable,  and  a  simplified  3-degree-of-freedom  model  is 
used  to  represent  the  air  vehicle.  Attempts  have  been  made  at  considering  3-dimensional 
scenarios  and  using  a  full  6-degree-of-freedom  air  vehicle  model,  but  these  efforts  typically 
revert  to  2-dimensional  approaches  when  faced  with  the  task  of  obstacle  avoidance  [9]. 
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2.3.3  Controllability  Matrices  and  Grammians.  We  typically  consider  state  space 
representations  of  linear  (or  linearized)  systems  to  be  given  in  the  following  form 

x(t)  =  Ax(t )  +  Bu(t ) 

y(t )  =  Cx(t )  +  Du(t )  (2.36) 

where  x(f)  is  the  length-?}  state  vector,  n(t)  is  the  length-m  input  vector  and  y(t)  is  the 
length-p  output  vector.  The  A,  B,  C  and  D  matrices  are  the  appropriately  sized  matrices 
which  describe  the  input-output  behavior  of  the  system  relative  to  the  internal  state  vector. 
In  general,  the  system  matrices  are  functions  of  time,  but  for  the  purpose  of  this  research, 
we  have  restricted  our  attention  to  constant  matrices. 

One  primary  concern  for  control  engineering  is  to  answer  the  question  [14],  "Starting 
from  the  origin  in  our  state-space,  can  I  drive  the  state  to  any  desired  final  location  in 
state-space  by  applying  a  control  input?"  If  the  answer  to  this  controllability  question  is 
yes,  then  the  system  is  considered  to  be  completely  controllable.  On  the  other  hand,  if 
the  answer  is  no,  then  the  system  is  said  to  be  uncontrollable  and  a  secondary  concern  is 
created.  For  an  uncontrollable  system,  we  seek  to  determine  the  regions  of  state-space 
we  can  in  fact  reach.  Finally,  for  both  the  completely  controllable  and  the  uncontrollable 
systems,  we  must  consider  the  relative  cost,  in  terms  of  control  input  energy,  required  to 
travel  along  the  various  directions  of  state-space.  This  last  piece  brings  in  a  quality  aspect 
to  the  controllability  discussion.  We  shall  now  investigate  these  concerns. 

The  mathematical  equivalent  of  the  controllability  question  is  the  desire  to  know  if 

x  €  (2.37) 

is  satisfied  for  all  possible  x,  where  Mc  is  a  special  matrix  known  as  the  controllability 
matrix,  3R{ - }  denotes  the  range  space  of  the  bracketed  matrix,  and  x  is  the  state  at  which 
we  wish  to  arrive,  having  started  from  the  zero  state.  This  controllability  matrix,  of 
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dimension  n  x  nrn.  is  computed  from  the  system  matrices  in  the  following  manner  [7]: 


Mc  = 


B  :  AB  :  A2B  An~1B 


(2.38) 


The  system  is  deemed  completely  controllable  if  and  only  if  Mc  is  of  rank  n.  In  the  event 
that  the  specified  system  is  uncontrollable,  the  reachable  region  of  state-space  may  be 
obtained  by  computing  the  range  space  of  Mc.  Conversely,  if  one  is  interested  in  explicitly 
specifying  the  unreachable  portion  of  state-space,  then  this  is  given  by  the  null  space  of 
Mc.  Methods  for  performing  these  two  computations  may  be  found  in  most  texts  on  linear 
systems  or  linear  algebra  [14, 18]. 

Having  addressed  the  first  two  issues  of  controllability,  we  now  consider  the  energy 
cost  required  to  drive  the  system  along  certain  directions  of  state-space.  A  convenient 
construct  to  perform  this  investigation  already  exists  in  the  form  of  the  controllability 
Grammian.  For  this  presentation,  we  will  need  to  make  use  of  the  state  transition  matrix, 
which  may  be  calculated  for  the  case  of  a  time-invariant  system  via  the  following  equation: 


$(t  -  to)  =  eAit~to) 


(2.39) 


where  to  is  the  initial  time.  The  controllability  Grammian  ( Wc)  is  then  defined  as  [12] 

Wc(t)  =  f  <S>{t-T)BBT<$>T(t-T)dT  (2.40) 

Jo 

or  in  an  alternate,  yet  equivalent,  form  as  [14] 

Wc{t)  =  $(t)  Qf  <S>(-T)BBT<S>T(-T)dT^J  4>T(f)  (2.41) 

and  has  the  following  properties  [14] 

1.  ITc(t)  is  symmetric  for  all  t  >  0. 

2.  All  of  the  eigenvalues  of  Wc{t)  are  either  positive  or  zero. 

3.  The  system  is  completely  controllable  if  and  only  if  Wc(t)  is  of  rank  n  for  t  >  0. 
This  is  equivalent  to  requiring  Wc(t)  to  have  only  positive  eigenvalues. 
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4.  Wc(t)  =  fg  Q(rj)BBT  <&T  (rj)dri  (note  that  this  obtained  by  substituting  rj  =  t  —  r). 

5.  Wc(t)  is  the  solution  to  the  n  x  n  matrix  Lyapunov  differential  equation  with  zero 
boundary  conditions 

=  AWc(t)  +  Wc(t)AT  +  BBt  (2.42) 

at 

6.  If  the  system  is  stable,  then  the  steady-state  controllability  Grammian,  WCgs,  exists 
and  is  the  solution  to  the  n  x  n  matrix  Lyapunov  equation 

0  =  AWCss  +  WCssAt  +  BBt  (2.43) 


7.  To  arrive  at  the  desired  state,  x,  the  norm  squared  value  of  the  minimum  energy 
control  satisfies 

||«*||2  <  t— ||®||2  (2-44) 

Amin 

where  A min  is  the  minimum  eigenvalue  of  Wc(t). 


8.  To  arrive  at  the  state  given  by  x  =  a^.,  where  is  the  unit  norm  eigenvector  of 
Wc(t)  corresponding  to  the  eigenvalue  A j,  the  norm  squared  value  of  the  minimum 
energy  control  is  given  by 


at 


m  = 


(2.45) 


From  the  final  property  in  this  list,  we  can  see  the  beginnings  of  how  a  concept 
of  quality  of  controllability  may  be  realized.  The  best  method  to  describe  this  is  with 
an  example.  Let  us  assume  that  the  eigenvalues  for  a  2  x  2  steady-state  controllability 
Grammian  are  Ai  =  4  and  A2  =  0.25.  The  energy  of  the  minimum  effort  control  for 
x  =  is  ||u*||2  =  0.25,  while  ||u*||2  =  4  for  the  case  of  x  =  £  .  This  demonstrates  that 
movement  in  the  £2  direction  of  state-space  is  sixteen  times  as  costly  as  movement  in  the  £ 
direction.  Thus,  even  though  it  is  possible  to  move  in  the  £2  direction,  it  is  rather  difficult 
to  accomplish  this  movement,  relative  to  the  effort  required  to  move  in  the  £  direction. 
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2.3.4  Minimum  Effort  Control.  Given  the  mathematical  construct  of  the  control¬ 
lability  Granrnrian,  we  may  turn  our  attention  to  the  concept  of  minimum  effort  control. 
The  goal  of  this  control  technique  is  to  find  the  input,  «*(f),  that  drives  the  zero  state 
response 

<h(— t)Bu{t)(It  (2-46) 

to  the  desired  state  x  within  a  finite  amount  of  time,  such  that  the  norm  of  this  input 
is  smaller  than  the  norm  of  any  other  input  that  can  reach  the  solution  xzs(tf)  =  x.  A 
control  input  that  attains  this  final  state,  one  that  has  been  proven  to  have  the  minimum 
norm,  is  given  by  [14] 


Xzsitf)  =  $(*/)  f 
Jo 


u*(t )  =  BT<S>T(tf  -  t)Wfl{tf)x,  0  <t<tf  (2.47) 


2-4  F-16  Simulation  Model 

The  final  topic  in  this  presentation  of  background  information  is  a  discussion  of  the 
F-16  dynamics  model  which  provides  the  basis  for  the  simulation  environment.  This 
simulation  model  was  developed  by  the  faculty  of  the  Air  Force  Institute  of  Technology 
(in  collaboration  with  Environmental  Tectronics  Corporation)  [11]  and  is  based  on  the  air 
vehicle  and  controller  models  described  in  [17]. 

Figure  2.5  shows  the  overall  setup  of  the  non-linear  system,  to  include  the  aircraft 
plant  model  and  the  feedback  autopilot  controllers.  At  the  core  of  this  simulation  is 
a  function  which  applies  saturation  limits  on  the  inputs  and  numerically  computes  the 
derivative  of  the  state  vector.  The  state  and  control  input  vectors  for  this  model  are 
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defined  as 


Vt 

&aoa 

P 


X  = 


e 

P 

Prate 

9rate 

Prate 

Pn 

Pe 

h 


throttle 


u  = 


('l&cmd 

ullcrnd 


r  dr  cmd 


(2.48) 


engine  lag 
d^-defl 

aildefl 

rdrdefi 

where  Vt  is  the  vehicle’s  air  speed,  aaoa  is  the  angle  of  attack,  f3  is  the  sideslip  angle,  p,  9, 
and  ip  are  the  roll,  pitch,  and  yaw  orientation  angles,  prate,  9rate ,  and  prate  are  the  rates 
of  change  of  the  orientation  angles,  pn  and  pe  are  the  northward  and  eastward  position, 
h  is  the  altitude,  engine  lag  is  a  lag  state  associated  with  the  engine  thrust  dynamics, 
and  eledefi,  aildefl,  and  rdrdefi,  are  the  actuator  deflection  angles  for  the  elevator,  aileron, 
and  rudder.  The  angle  of  attack  and  the  sideslip  angle  describe  the  orientation  of  the 
vehicle  with  respect  to  the  velocity  vector  and  are  two  of  the  angles  which  help  to  define 
the  direction  of  the  vehicle’s  motion.  In  the  control  input  vector,  throttle  is  commanded 
throttle  setting  expressed  as  a  percentage  of  the  maximum  value,  and  elecmd ,  ctilcmd ,  and 
rdrcmd  are  the  commanded  deflection  angles  for  the  elevator,  aileron,  and  rudder. 


Each  of  the  feedback  controllers  in  Figure  2.5  is  a  simple  autopilot  designed  to  main¬ 
tain  a  predetermined  set  of  equilibrium  velocity  and  orientation  conditions.  External 
inputs  are  available  to  modify  the  feedback  commands  in  an  effort  to  mimic  the  inputs 
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Figure  2.5  System  model  of  an  F-16  aircraft  and  autopilot  controllers 


of  a  pilot.  This  model  was  originally  created  to  evaluate  the  forces  sensed  at  the  pilot’s 
seat  [11]  and,  as  such,  the  linear  (lin_accel)  and  rotational  (rot_accel)  accelerations  shown 
in  Figure  2.5  are  holdovers  from  the  research  which  previously  used  this  model. 

In  order  to  facilitate  the  development  in  Chapter  3,  we  must  detail  the  two  angles 
which  define  the  vehicle’s  velocity  vector.  These  two  angles,  known  as  the  flight  path  angle, 
Ipathi  and  the  heading  angle,  'k,  angles  are  shown  in  Figure  2.6  and  may  be  calculated  using 
the  information  from  the  state  vector  that  defines  the  system  in  Figure  2.5.  The  flight  path 
angle  is  what  actually  determines  whether  the  munition  is  climbing  or  diving.  Knowledge 
of  the  angle  of  attack  and  the  vehicle  pitch  angle,  both  of  which  may  be  measured,  permits 
us  to  determine  the  flight  path  angle.  Mathematically,  the  flight  path  angle  is  computed 
as 

Ipath  @  CVaoa  (2.49) 

as  seen  in  Figure  2.6.  The  heading  angle  describes  the  motion  of  the  vehicle  within  the 
horizontal  plane  and  may  be  determined  from  the  yaw  orientation  angle  and  the  sideslip 
angle  using  the  following  expression 


T  =  i\)  —  (3 


(2.50) 


also  as  seen  in  Figure  2.6.  Note  the  difference  between  T,  the  heading  angle,  and  ip,  the 
vehicle  yaw  angle.  Given  these  two  angles  and  the  air  speed  of  the  vehicle  we  may  define 
the  north,  east,  and  vertical  velocities  as 


Vnorth, 

Krcos(T)cos(7path) 

Veast 

= 

Fr  sin('F)  cos(7pat/J 

^ vertical 

VT  sm(-/path) 

2.5  Summary 

The  various  sections  of  this  chapter  have  provided  the  fundamental  concepts  required 
to  proceed  with  this  research.  First,  Section  2.2  provided  the  background  of  linear  Kalman 
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Figure  2.6  Description  of  flight  path  and  heading  angles 

filtering,  the  popular  method  used  in  many  target  tracking  applications.  Next,  Section 
2.3  detailed  a  few  methods,  including  circle-line-circle  control,  model  predictive  control, 
and  minimum  effort  control,  to  accomplish  an  optimal  control  task  in  two  dimensions. 
Finally,  Section  2.4  presented  a  basic  simulation  environment  based  on  the  flight  dynamics 
of  an  F-16  aircraft.  Equipped  with  this  foundation,  we  now  turn  to  the  development  of  a 
3-dimensional  guidance  algorithm  and  the  extension  of  the  control  optimization  techniques 
discussed  in  Section  2.3. 
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III.  Algorithm  Development  and  Simulation  Description 

3.1  Introduction 

In  this  chapter  we  present  the  development  of  the  guidance  algorithm  and  the  ex¬ 
tension  of  the  control  optimization  techniques  to  three  dimensions.  As  shown  in  Figure 
3.1,  the  guidance  algorithm  consists  of  the  target  tracking,  waypoint  generation,  and  path 
selection  subroutines.  When  appropriate,  pseudocode  will  be  provided  and  an  example 
will  be  discussed  in  the  text.  Additionally,  the  rationale  behind  major  decisions  will  be 
explained.  The  target  information  block  of  Figure  3.1  is  merely  an  external  input  which 
provides  the  target  measurement  data  required  by  the  target  tracker.  We  are  now  ready 
to  step  through  the  remaining  sections  of  Figure  3.1  sequentially,  starting  with  the  target 
tracker. 

3.2  Target  Tracker 

The  primary  function  of  the  target  tracker  in  Figure  3.1  is  to  determine  the  target 
with  the  highest  value  and  to  compute  an  aimpoint  for  the  munition.  In  order  to  accom¬ 
plish  these  tasks,  the  tracker  will  have  to  maintain  accurate  position  and  velocity  estimates 
for  each  potential  target.  Additionally,  we  will  require  some  form  of  attribute  data  so  that 
the  tracker  can  classify  the  potential  targets  according  to  a  list  of  target  types. 

The  linear  Kalman  filter  of  Equations  (2.4)  and  (2.6)  forms  the  basis  of  this  target 
tracker.  Realizing  that  we  intend  to  engage  ground  targets,  we  use  the  constant  velocity 
model,  with  $  and  Qd  matrices  given  by  Equation  (2.11),  to  describe  the  kinematics  of 
the  targets  in  the  scenario.  For  simplicity,  each  target  in  this  multi-target  scene  will 
be  tracked  independently,  and  only  the  measurement  corresponding  to  the  current  target 
under  consideration  will  be  presented  to  the  Kalman  filter.  The  estimate  that  we  obtain 
(x)  from  this  tracking  filter  only  contains  information  regarding  the  position  and  velocity 
of  the  target.  An  attribute  tracking  method  is  required  to  provide  the  determination  of 
target  type.  A  hard  decision  on  target  type  is  not  necessary,  and  it  is  possible  to  make 
soft  assignments,  such  that  we  obtain  the  probability  of  the  target  being  a  specific  type. 
As  will  be  shown  in  the  following  discussion  on  target  values,  the  soft  assignment  of  target 
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Figure  3.1  Flow  diagram  of  simulation 
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type  is  preferred  since  it  permits  the  computation  of  a  blended  target  value.  In  order 
to  ensure  we  only  engage  enemy  targets,  friendly  and  neutral  targets  would  have  to  be 
considered  obstacles  to  be  avoided  by  the  remainder  of  the  path  planning  process. 

Given  our  desire  to  engage  the  highest  value  target,  we  must  now  consider  how  to 
assign  target  values.  Two  obvious  possibilities  for  computing  target  value  are  a  maximum 
a  posteriori  value  computation 

v\  =  max  ur:j  (3-1) 

Pt . 

3 

and  a  probability-weighted  value  computation 

Nt 

V2  =  ^PTj^Tj  (3.2) 

3= 1 

where  vl  is  the  value  of  target  i,  plT.  is  the  probability  that  target  i  is  of  type  Tj ,  ur3 
is  the  value  of  target  type  Tj,  and  Nt  is  the  number  of  target  types.  The  benefit  of 
the  probability-weighted  target  value  computation  method  is  that  it  allows  for  a  more 
refined  estimate  of  the  target’s  identification  by  incorporating  the  uncertainty  in  target 
classification  function  directly  into  the  target’s  value.  This  also  permits  a  more  definitive 
determination  of  the  highest-valued  target  since  the  value  of  individual  targets  will  tend 
to  be  more  spread  out  than  if  the  maximum  a  posteriori  value  computation  method  was 
implemented. 

Finally,  once  we  have  assigned  a  value  to  all  the  targets  in  the  scenario,  we  have  to 
determine  a  terminal  waypoint  for  the  munition,  wp 'aim-  One  logical  option  for  computing 
the  aimpoint  would  be  the  value- weighted  centroid  of  the  targets 

N 

wp  .  =  >  Vpos.  (3.3) 

— —aim  /  - - 1  v  J 

i=  1 

where  vl  is  the  normalized  value  for  target  i  and  pos.  is  the  estimated  position  for  this 
same  target. 
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3.3  Waypoint  Generator 

This  operation  establishes  the  set  of  candidate  paths  that  is  passed  to  the  path 
selection  algorithm.  Each  path  is  determined  by  placing  a  series  of  waypoints  capable  of 
guiding  the  munition  toward  the  given  termination  point.  The  candidate  paths  generated 
in  this  section  are  determined  in  a  fashion  that  guarantees  obstacle  avoidance,  at  least  in 
terms  of  the  guidance  path.  We  assume  that  the  vehicle’s  path  planner  has  knowledge  of 
the  location  of  all  obstacles  in  the  scenario,  as  stated  in  Chapter  1,  and  that  the  obstacles 
may  be  represented  as  simple  shapes  (primarily  rectangular  solids).  The  use  of  simple 
shapes  is  a  highly  non-restrictive  assumption  since  complex  shapes  (such  as  ellipsoids)  may 
be  represented  by  a  circumscribed  box. 

First  we  must  determine  if  the  munition  would  impact  any  obstacles  while  flying  the 
line-of-sight  path  to  the  target.  This  is  accomplished  by  computing  the  vehicle’s  position 
at  numerous  sample  points  along  this  LOS  vector  and  then  checking  if  any  position  triplet 
falls  within  the  bounds  of  the  obstacles.  If  we  determine  that  no  impacts  occur,  then  this 
entire  path  planning  process  may  be  bypassed  and  the  optimal  path  is  found  to  be  simply 
the  LOS  path  between  the  munition  and  the  target.  On  the  other  hand,  for  the  more 
important  case,  in  which  we  must  maneuver  around  an  obstacle,  we  need  to  place  a  series 
of  waypoints  to  guide  the  munition  past  the  obstacle. 

The  simple  2-dimension  version  of  this  problem  tells  us  to  plan  three  paths.  The  first 
path  maintains  the  line-of-sight  heading  while  flying  up-and-over  the  obstacle.  The  other 
two  paths  maintain  the  current  altitude  while  flying  directly  around  the  obstacle.  Even 
though  these  three  paths  would  be  sufficient  if  we  merely  want  to  get  past  the  obstacle, 
our  desire  to  accomplish  this  task  in  an  efficient  manner  drives  us  to  formulate  additional 
path  options.  For  illustrative  purposes,  we  choose  to  create  two  more  candidate  paths 
somewhere  in  between  the  directly  over  and  directly  around  paths.  The  obvious  option  is  to 
fly  at  a  heading  halfway  between  the  up-and-over  and  the  around  paths  while  selecting  the 
altitude  of  the  waypoint  based  on  the  height  of  the  obstacle.  A  notional  set  of  waypoints 
have  been  placed  in  Figure  3.2.  Note  that  the  paths  shown  do  not  actually  impact  the 
obstacle,  but  are  offset  from  the  edges  by  a  small  safety  buffer.  Given  the  type  of  path 
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First  Waypoint  Set 


Figure  3.2 


Notional  waypoint  placement  -  paths  do  not  impact  obstacle 


(up-and-over,  around,  up-and-around)  and  the  dimensions  of  the  obstruction,  we  are  now 
ready  to  place  the  waypoints  for  the  5  candidate  paths. 


For  the  simplest  path,  the  up-and-over  candidate,  we  place  the  altitude  of  the  way- 
point  at  the  maximum  altitude  of  the  obstacle  we  are  projected  to  impact  and  add  in  a 
user-defined  safety  buffer.  The  north  and  east  position  of  the  waypoint  are  then  fixed 
to  be  equal  to  the  location  where  we  initially  strike  the  obstacle.  This  gives  us  the  first 
waypoint  choice  as 

Prior  th, impacts 


W  Pup— over  — 


P  east  , impacts 


(3.4) 


hmax  +  dsajefy 


where  Vnorth,impact0  and  peast,imPact0  denote  the  north  and  east  position  of  the  point  of 
impact  on  the  first  obstacle  we  reach,  hrnax  is  the  maximum  altitude  of  this  obstacle,  and 
d safety  is  the  adjustable  safety  margin. 


For  the  around  paths,  our  specific  placement  of  waypoints  is  dependent  upon  both 
our  general  heading  and  our  starting  position  relative  to  the  obstacle’s  boundary  in  the 
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horizontal  plane.  For  instance,  if  the  heading  of  the  line-of-sight  path  to  the  next  waypoint 
is  primarily  in  a  northward  direction  and  if  the  previous  waypoint  is  located  to  the  west  of 
the  obstacle,  then  we  should  attempt  to  fly  around  the  obstacle  by  choosing  the  northwest 
and  southeast  corners  of  the  obstacle  as  intermediate  waypoints.  As  stated  earlier,  we 
maintain  the  altitude  of  the  previous  waypoint  for  these  two  new  waypoints.  A  summary 
of  the  entire  decision  logic  is  shown  in  Algorithm  1. 

In  the  case  of  the  up-and-around  paths,  we  begin  by  computing  our  desired  heading, 
up-around;  as  the  angle  which  bisects  the  up-and-over  and  around  paths 

lTf  _  ^  up— over  A  ’ll around  ~  ^ 

^  up— around  ^ 

Next,  we  determine  the  vehicle’s  position  at  various  sample  points  along  this  new  heading 
and  obtain  the  same  type  of  obstacle  impact  information  that  was  used  for  the  up-and-over 
path.  Specifically,  we  compute  the  location  of  first  impact  and  the  maximum  height  of 
the  obstacle.  We  finish  this  path  type  by  computing  the  location  of  the  waypoint  in  the 
same  manner  as  Equation  (3.4). 

Now  that  we  have  determined  a  set  of  waypoints  for  the  front  edge  of  an  obstacle, 
a  series  of  verifications  must  be  made.  In  the  first  verification  step,  we  check  to  see  if 
the  LOS  path  from  the  new  waypoint  to  the  termination  point  passes  through  the  same 
obstacle  we  are  attempting  to  avoid.  If  an  impact  is  detected,  then  we  are  forced  to  place 
an  intermediate  waypoint  based  on  the  type  of  the  path  (up-and-over,  around,  up-and- 
around)  we  are  extending  and  the  general  heading  of  this  LOS  path.  As  an  example,  if 
we  must  extend  an  up-and-over  path  and  we  are  heading  in  a  northerly  direction,  then  we 
shall  hold  the  altitude  constant  and  set  the  north  position  of  the  intermediate  waypoint 
on  the  northern  edge  of  the  obstacle  (plus  the  adjustable  safety  distance).  Although 
this  approach  may  appear  to  cause  issues  in  the  presence  of  multi-modal  obstacles,  the 
final  layer  of  verification  in  this  series  will  eliminate  this  concern.  The  east  position  of 
this  waypoint  is  then  computed  using  simple  planar  geometry.  As  shown  in  Figure  3.3, 
the  east  position  is  given  as  peast,new  =  Peast,oid  +  k-north  tan(’F).  The  following  three 
sections  of  pseudocode  present  the  method  used  to  extend  the  up-and-over  (Algorithm  2), 
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Algorithm  1  Compute  waypoints  of  "around"  type 
poso  =  position  of  previous  waypoint 
Set  altitude  of  waypoint  at  altitude  of  poso 
if  Heading  north  then 

if  poso  is  west  of  obstacle  then 

5:  Place  waypoints  on  northwest  and  southeast  corners  of  obstacle 

else  if  posq  is  east  of  obstacle  then 

Place  waypoints  on  northeast  and  southwest  corners  of  obstacle 
else 

Place  waypoints  on  southwest  and  southeast  corners  of  obstacle 

10:  end  if 

else  if  Heading  south  then 
if  posq  is  west  of  obstacle  then 

Place  waypoints  on  southwest  and  northeast  corners  of  obstacle 
else  if  posq  is  east  of  obstacle  then 
15:  Place  waypoints  on  northwest  and  southeast  corners  of  obstacle 

else 

Place  waypoints  on  northwest  and  northeast  corners  of  obstacle 

end  if 

else  if  Heading  east  then 
20:  if  posq  is  north  of  obstacle  then 

Place  waypoints  on  northeast  and  southwest  corners  of  obstacle 
else  if  poso  is  south  of  obstacle  then 

Place  waypoints  on  northwest  and  southeast  corners  of  obstacle 
else 

25:  Place  waypoints  on  northwest  and  southwest  corners  of  obstacle 

end  if 
else 

if  poso  is  north  of  obstacle  then 

Place  waypoints  on  northwest  and  southeast  corners  of  obstacle 
30:  else  if  poso  is  south  of  obstacle  then 

Place  waypoints  on  northeast  and  southwest  corners  of  obstacle 
else 

Place  waypoints  on  northeast  and  southeast  corners  of  obstacle 

end  if 
35:  end  if 
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around  (Algorithm  3),  and  up-and-around  (Algorithm  4)  waypoints.  The  extension  of  the 
waypoints  shown  in  Figure  3.2  is  seen  in  Figure  3.4. 


Algorithm  2  Extending  "up-and-over"  paths 
poso  =  position  of  previous  waypoint 
To  =  heading  of  path  at  poso 
Set  altitude  of  waypoint  at  altitude  of  poso 
if  Heading  north  then 
5:  Follow  To  to  north  edge  of  obstacle 

else  if  Heading  south  then 

Follow  To  to  south  edge  of  obstacle 
else  if  Heading  east  then 

Follow  To  to  east  edge  of  obstacle 
10:  else 

Follow  To  to  west  edge  of  obstacle 

end  if 


Algorithm  3  Extending  "around"  paths 
poso  =  position  of  previous  waypoint 
Set  altitude  of  waypoint  at  altitude  of  poso 
if  Heading  north  then 

Extend  waypoints  to  northern  corners  of  obstacle 
5:  else  if  Heading  south  then 

Extend  waypoints  to  southern  corners  of  obstacle 
else  if  Heading  east  then 

Extend  waypoints  to  eastern  corners  of  obstacle 
else 

10:  Extend  waypoints  to  western  corners  of  obstacle 

end  if 


The  second  level  of  verification  is  only  invoked  if  an  intermediate  waypoint  was 
generated  as  a  result  of  the  first  layer  of  checks.  At  this  point  we  are  still  concerned 
with  impacting  the  same  obstacle,  even  though  we  do  not  expect  there  to  be  any  further 
trouble  with  this  obstacle.  If  an  obstacle  collision  is  detected  on  the  LOS  path  between 
the  intermediate  waypoint  and  the  waypoint  from  which  it  was  extended,  then  this  entire 
candidate  path  may  be  discarded  as  a  dead-end. 

The  final  verification  layer,  which  is  always  necessary,  checks  for  obstacle  crossings 
on  the  LOS  between  the  most  recently  generated  waypoint  and  the  terminal  waypoint.  If 
no  collision  is  detected,  then  this  candidate  path  is  complete.  On  the  other  hand,  when 
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Algorithm  4  Extending  "up- and- around"  paths 


poso  =  position  of  previous  waypoint 
1  path :o  =  flight  path  angle  at  pos0 
'ho  =  heading  angle  at  poso 
if  Heading  north  then 

5:  Extend  waypoints  along  'ho  and  'ypath,o  to  northern  edge  of  obstacle 

else  if  Heading  south  then 

Extend  waypoints  along  *ho  and  7 path,o  t°  southern  edge  of  obstacle 
else  if  Heading  east  then 

Extend  waypoints  along  'ho  and  'ypath,o  t°  eastern  edge  of  obstacle 
10:  else 

Extend  waypoints  along  'ho  and  7 path,o  t°  western  edge  of  obstacle 

end  if 


Figure  3.3  Waypoint  extension 
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Figure  3.4  Extension  of  notional  waypoints 

a  collision  with  a  different  obstacle  is  detected,  the  waypoint  generation  process  must  be 
repeated  using  the  most  recent  waypoint  as  the  starting  position. 

This  entire  process  in  repeated  until  all  candidate  paths  are  designated  as  either 
completed  or  dead-ends.  As  one  can  see,  this  algorithm  has  the  potential  of  generating  up 
to  5n°bat  candidate  paths,  where  n0bst  is  the  number  of  obstacles  between  the  munition’s 
launch  point  and  the  target’s  location.  The  full  waypoint  generation  process  is  summarized 
in  Algorithm  5. 

3-4  Path  Selection 

The  role  of  the  path  selection  block  in  Figure  3.1  is  to  determine  the  optimal  obstacle- 
free  path  from  a  given  set  of  potential  flight  paths.  Minimum  effort  control  theory  provides 
the  basis  for  making  this  determination.  This  algorithm  is  also  capable  of  identifying  paths 
that  are  not  feasible  to  traverse  for  the  set  of  given  flight  conditions.  A  cost,  made  up 
of  three  components,  is  computed  for  each  candidate  path  and  has  the  following  general 
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Algorithm  5  Top-level  Waypoint  Generation 
Check  for  obstacle  collisions 
if  collision  detected  then 
Generate  set  of  5  waypoints 
for  all  new  waypoints  do 

5:  Check  for  obstacle  collisions  on  same  obstacle  along  LOS  path  to  aimpoint 

if  collision  detected  then 
Extend  the  waypoint 

Check  for  obstacle  collisions  on  same  obstacle  along  LOS  path  from  previous 
waypoint 

if  collision  detected  then 
10:  Delete  candidate  path 

end  if 
end  if 

Check  for  obstacle  collisions  on  new  obstacle  along  LOS  path  to  aimpoint 
if  collision  detected  then 

15:  Initiate  new  instance  of  waypoint  generation 

else 

Declare  candidate  path  complete 

end  if 
end  for 
20:  end  if 


form: 


Jpat.h  —  J orientation  T  J travel  T  J distance 


(3-6) 


The  explanation  of  each  component  of  this  cost  will  be  given  later  in  this  section.  As 
an  innovation  of  this  research,  these  component  costs  are  chosen  based  on  the  way  the 
problem  is  decomposed.  Additionally,  early  test  cases  indicated  that  the  north  and  east 
position  states  from  the  state  vector  of  Equation  (2.48)  led  to  an  unstable  system.  Since 
the  vehicle  is  assumed  to  fly  at  a  constant  speed,  one  cannot  expect  the  munition  to  reach 
a  steady-state  value  for  north  and  east  position.  The  data  provided  by  these  two  states 
must  be  ignored  for  the  purpose  of  computing  controllability  Grammians.  Given  this  loss 
of  important  information,  Jdistance  is  an  effort  to  address  the  fact  that  a  portion  of  the 
expended  energy  will  be  proportional  to  the  total  distance  travelled.  The  candidate  path 
with  the  lowest  total  cost  is  then  selected  as  the  desired  set  of  waypoints  for  the  munition 
to  follow  in  order  to  reach  the  target.  We  will  now  outline  the  general  procedure  to  be 
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followed  in  computing  the  orientation  and  travel  costs,  starting  with  a  brief  discussion  of 
the  controllability  Grammian  concept  which  is  shared  by  both  of  these  component  costs. 

First,  a  linearized  system  model  of  the  air  vehicle  itself  is  generated.  This  linearized 
state-space  model  is  obtained  by  numerically  computing  the  Jacobian  matrix  [11]  for  the 
six-degree-of-freedom  F-16  aircraft  plant  model  given  in  Figure  2.5  and  evaluating  this 
Jacobian  at  a  given  set  of  trimmed  flight  conditions.  These  trim  conditions  are  assumed 
to  be  the  equilibrium  state  values  and  the  equilibrium  input  values  required  to  fly  the 
vehicle  in  a  specified  manner,  such  as  steady-level  flight  or  wings-level  nose-up  (or  nose- 
down)  flight.  Next,  the  feedback  control  loops  are  closed  in  order  to  form  the  full  linearized 
state-space  model  of  the  system  under  consideration.  With  this  linearized  model  available, 
we  finally  compute  the  steady-state  controllability  Grammian  using  Equation  (2.43)  for  the 
total  system  under  the  current  equilibrium  state  and  input  conditions.  A  cost  may  now  be 
computed  as  the  norm  squared  value  of  the  minimum  energy  control,  in  a  manner  similar 
to  Equation  (2.45),  required  to  reach  a  certain  set  of  state  values 


w  = 


E 

i=  1 


aj 


(3.7) 


where  A,;  is  the  ith  eigenvalue  of  the  controllability  Grammian  and  on  is  a  scale  factor 
associated  with  this  eigenvalue.  Note  that  this  is  an  extension  of  Equation  (2.45)  to  the 
case  of  a  more  generalized  desired  state  vector. 

Before  we  continue  to  detail  the  unique  steps  taken  to  compute  each  component 
cost,  we  must  give  a  proof  of  how  this  generic  cost  is  computed  from  the  controllability 
Grammian. 

Proof.  Let  us  assume  that  we  have  a  stable  system  described  in  state-space  by  the 
matrices  A,  B,  C,  and  D  (Equation  (2.36)).  Since  the  system  is  stable,  we  are  guaranteed 
that  the  steady-state  controllability  Grammian,  WCgs,  can  be  obtained  by  solving  Equation 
(2.43).  Furthermore,  let  us  also  assume  that  WCsg  is  of  full  rank  so  that  the  system  is 
completely  controllable  (property  3  of  controllability  Grammians).  This  last  assumption 
is  not  necessary  but  will  make  the  proof  a  bit  easier.  Later  in  the  path  selection  process, 
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a  modification  of  this  assumption  will  be  used  as  a  built-in  method  to  reduce  the  number 
of  candidate  paths. 

To  get  the  proof  started,  we  simply  compute  the  eigenvalues  and  unit-norm  eigen¬ 
vectors  of  WCgg  .  From  linear  algebra,  we  know  that  the  eigenvalues  for  a  real,  symmetric 
matrix  are  guaranteed  to  be  real  and  that  the  eigenvectors  for  distinct  eigenvalues  will  be 
orthogonal  to  each  other.  Now,  for  any  desired  state  x,  we  evaluate  the  scalar  projection 
of  x  along  each  £.  to  obtain  a  set  of  scale  factors  which  describe  the  amount  of  state  motion 
required  in  the  £.  direction  of  state-space 

a*  =  (3.8) 

Given  that  WCgg  is  real  and  symmetric  and  assuming  that  the  n  eigenvalues  are  all  dis¬ 
tinct  (this  is  not  an  unrealistic  assumption),  the  n  eigenvectors  are  known  to  be  linearly 
independent  of  each  other  and  we  may  write  the  desired  state  vector  as 

n 

x  =  J2  (3-9) 

i=  1 

Next,  we  solve  for  the  energy  of  the  optimal  control  input  vector  using  the  following 
norm  expression 

u*t(t)u*(t)cIt  (3.10) 

After  substituting  in  the  form  of  the  control  input  from  Equation  (2.47),  the  energy  of  this 
vector  is  given  by 

(3.11) 

where  we  have  applied  the  properties  of  the  state  transition  matrix  such  that  <hT(t/  —  r)  = 
<kT(  — r)&T(tf).  The  bracketed  term  in  Equation  (3.11)  is  the  steady-state  controllability 
Grammian  by  definition  (Equation  (2.41)).  Replacing  x  with  the  form  of  Equation  (3.9), 


nW-slmtf)  <P(-T)BBT$T(-T)dT<S>T(tf)}W- 
J  0 
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the  norm  squared  value  of  the  input  can  now  be  reduced  to 


«  = 


W-1  I  Y.a,^ 


(3.12) 


We  know  that  the  eigenvalues  and  eigenvectors  for  a  given  matrix  (fi)  are  related  to  each 
other  by  =  A [18]  and  it  is  easy  to  see  that  as  long  as  the  inverse  of 

tt  exists.  Applying  this  to  our  input  energy  computation,  the  energy  of  the  input  becomes 


u 


(3.13) 


Since  the  eigenvectors  are  presumed  to  all  be  mutually  orthogonal,  we  may  merge  the 
summations  to  obtain 


n  2 

|u*||2  = 

■  1 


(3.14) 


Noting  that  the  eigenvectors  used  in  this  problem  are  forced  to  be  of  unit  length  (£.  £  =  1), 
the  norm  squared  value  of  the  minimum  energy  control  required  to  reach  the  desired  state 
is  therefore  given  by  Equation  (3.7).  ■ 


It  must  be  noted  here  that  the  x  vector  used  throughout  the  preceding  proof  is  really 
the  desired  change  in  system  state  such  that,  for  a  system  linearized  about  some  nominal 
state 


ry*  -  _  rv» 

—  —desired  —nominal 


(3.15) 


We  are  now  ready  to  deal  with  the  specifics  of  each  component  cost. 

For  the  orientation  cost,  we  are  primarily  concerned  with  the  energy  required  to 
make  changes  in  the  vehicle’s  heading  and  flight  path  angles.  However,  given  that  the 
angle  of  attack  and  pitch  angle  will  vary  slightly  as  a  function  of  altitude,  this  component 
cost  will  also  consider  the  energy  needed  to  achieve  an  altitude  change.  These  energies  will 
be  referred  to,  respectively,  as  the  orientation  energy  (\\u*  \\^rient)  and  the  height  energy 
(lift* \\height)’  an<^  we  consider  each  in  turn. 
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In  the  case  of  the  orientation  energy,  the  computed  equilibrium  state  and  input 
values,  to  be  used  in  the  system  linearization  process,  correspond  to  the  air  vehicle’s  flight 
parameters  (speed,  altitude,  and  flight  path  angle)  at  the  current  waypoint.  The  state  at 
which  we  desire  arrive  is  identical  to  the  current  equilibrium  state  except  that  the  heading 
and  flight  path  angle  information  is  replaced  by  the  angles  required  to  turn  toward  the 
next  waypoint.  For  the  height  energy,  the  starting  equilibrium  conditions  are  the  state 
and  input  values  to  which  we  drove  the  system  while  computing  the  orientation  cost.  The 
goal  state  for  this  computation  is  merely  the  current  equilibrium  state  with  the  altitude 
of  the  next  waypoint  replacing  that  of  the  current  waypoint.  The  overall  orientation  cost 
for  the  Ith  candidate  path  is  given  by 


J, 


orientation, l 


=  U 


—  II  orient, l 


II2 

II  height, l 


(3.16) 


or  in  a  generalized  form  which  allows  for  relative  weighting  between  the  orientation  and 
height  energies 

j orientation,!  =  hi  Hit  II orient, l  +  W-2  \\u  || height, l  (3-17) 


For  the  travel  cost,  we  are  interested  in  addressing  the  energy  involved  in  merely 
flying  on  a  straight  and  level  path.  Bearing  this  in  mind,  the  equilibrium  values  used 
in  the  system  linearization  are  computed  based  on  steady-level  flight  at  a  constant  speed 
and  at  the  altitude  of  the  next  waypoint.  Due  to  our  desire  to  fly  straight  and  level  over 
a  given  distance  (d),  we  must  alter  the  energy  computation  to  account  for  this  distance 
traveled.  In  order  to  keep  the  computation  balanced  correctly,  we  must  also  modify  the 
state  vector  we  are  driving  toward.  After  incorporating  these  changes,  we  arrive  at  the 
following  equations  to  be  used  only  for  the  travel  cost  computations 


x  = 


E 

i—  1 


oiii. 


(3.18) 


instead  of  Equation  (3.9)  and 


|u*||2 

1-^  Wtravel  ~ 

2  =  1  1 


(3.19) 
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instead  of  Equation  (3.7).  The  overall  travel  cost  for  the  Ith  candidate  path  is  given  by 


•J travel) 


U 


1 1 2 

I  \  travel, l 


(3.20) 


The  distance  cost,  although  it  is  computed  based  on  the  distance  travelled,  is  really 
misnamed.  Since  we  have  made  the  assumption  that  the  vehicle  maintains  a  given  speed 
throughout  its  flight,  this  is  really  a  manner  of  incurring  a  cost  for  prolonged  flight  times. 
The  rationale  behind  this  cost  component  is  that,  lacking  an  explicit  expression  for  fuel 
consumption,  a  longer  flight  will  tend  to  require  more  propellent  than  a  shorter  flight. 
This  cost  is  based  on  calculating  the  total  distance  ( disttot ))  traveled  for  the  Ith  candidate 
path  as  shown  below 


disttot)  =  ^2 


i=  1 


wpi+1  -  wp. 


(3.21) 


where  nwp  is  the  number  of  waypoints  on  the  path  under  consideration 


The  procedure  described  above  is  used  to  compute  the  cost  to  fly  between  any  two 
waypoints  in  a  candidate  path.  Once  the  energies  and  distances  are  fully  computed  for 
each  candidate  path,  we  are  finally  ready  to  determine  the  component  costs  associated 
with  each  path.  In  order  to  ensure  that  the  values  are  all  around  the  same  rough  order  of 
magnitude,  we  will  simply  normalize  the  data  in  each  component,  so  that  we  arrive  at  the 
following  component  costs 


J orientation) 
J travel) 
J distance ) 


J, 


orientation) 


E^lpaths  j 

2—1  J  orientation,^ 

J  travel, l 

E^paths  j 

2=1  J  travel, i 

disttot) 


E-=“i  s  dist-, 


tot,i 


(3.22) 


where  npaths  is  the  total  number  of  candidate  paths  being  evaluated.  The  net  effect  of 
this  normalization  is  to  give  each  cost  component  an  equal  weighting  in  the  overall  cost 


computation 


Jpath)  —  J orientation)  T  Jtravel)  T  Jdistance) 


(3.23) 
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One  could  readily  alter  the  relative  weighting  of  these  costs  by  including  a  multiplicative 
factor  on  each  component  cost 


Jpath,l  —  W3J orientation, l  “I-  H  4. ) travel, l  T  ^^5  J  distance, l 


(3.24) 


This  cost  with  relative  weighting  factors  is  a  generalization  of  Equation  (3.6)  and  exem¬ 
plifies  the  fact  that  we  have  given  each  component  cost  an  equal  vote  in  determining  the 
total  path  cost. 

If,  at  any  point  in  the  procedure  delineated  above,  the  linearized  system  is  found 
to  be  unstable,  then  the  entire  candidate  path  under  consideration  at  that  instant  may 
immediately  be  discarded.  In  addition,  if  the  steady-state  controllability  Grammian  in¬ 
dicates  that  the  system  is  not  completely  controllable,  as  evidenced  by  a  zero  eigenvalue, 
and  we  desire  the  system  to  move  in  an  uncontrollable  direction  of  state-space,  as  seen  by 
a  non-zero  cti  value  corresponding  to  the  zero  eigenvalue,  then  this  candidate  path  may  be 
ruled  out  based  on  the  infeasibility  of  arriving  at  one  of  the  waypoints.  This  entire  path 
selection  procedure  is  summarized  in  Algorithm  6. 


Algorithm  6  Path  Selection  Algorithm 

for  i  =  1  to  number  of  candidate  paths  do 
if  candidatei  exists  then 

for  j  =  1  to  number  of  waypoints  in  candidatei  —  1  do 

if  system  unstable  at  waypoint  j  or  waypoint  j  is  in  uncontrollable  space  then 
5:  Ignore  candidatei 

else 

Compute  cumulative  energy  for  orientation  and  height  changes  for  candidatei 
Compute  cumulative  energy  to  travel  on  candidatei 
Compute  cumulative  distance  travelled  on  candidatei 

10:  end  if 

end  for 
end  if 
end  for 

Normalize  orientation  energy,  travel  energy,  and  distance  for  each  candidate  path 
15:  Compute  total  cost  for  each  candidate  path 
Select  path  with  minimum  cost 
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3.5  Command  Generator 


The  function  of  this  block  is  to  generate  the  optimal  input  which  will  drive  the 

vehicle  to  the  next  waypoint  under  the  assumption  that  we  wish  to  fly  along  the  line-of- 

sight  path  as  much  as  possible.  Though  there  are  numerous  optimal  control  methods,  this 

research  is  concerned  with  extending  the  work  on  CLC  [1]  and  MPC  [9]  control  schemes  to 

3-dimensional  problems  and  presenting  a  minimum  effort  control  technique  which  makes 

use  of  data  obtained  during  the  path  selection  procedure.  While  all  three  of  these  control 

optimization  approaches  require  the  specification  of  a  set  of  waypoints,  only  MPC  imposes 

the  additional  need  to  generate  a  reference  path  between  the  waypoints.  The  general 

procedure  for  these  three  command  generation  approaches  is  described  here  and  the  details 
(R) 

of  a  MATLAE>  implementation  of  the  MPC  and  minimum  effort  control  techniques  will 
be  given  in  Chapter  4. 

3.5.1  Extremal  Input  Commanding.  Recall  that  within  the  CLC  optimal  con¬ 
trol  technique  is  the  desire  to  minimize  the  deviation  from  the  line-of-sight  path  between 
waypoints  while  operating  under  the  assumption  that  all  turns  are  made  at  the  vehicle’s 
maximum  rate  of  turn.  In  fact,  this  control  optimization  method  does  not  directly  com¬ 
pute  any  control  inputs;  instead  the  CLC  method  determines  the  time  to  apply  one  of  a 
given  set  of  inputs.  The  specific  control  inputs  that  make  up  this  set  of  potential  inputs 
are  completely  known  a  priori ,  and  the  chosen  input  is  based  on  which  direction  the  vehicle 
is  attempting  to  turn.  However,  this  simple  approach  is  only  valid  for  a  2-dimensional 
problem.  In  this  section,  we  present  the  extension  to  a  3-dimensional  scenario  and  attempt 
to  show  where  this  process  breaks  down. 

First,  we  extend  the  fundamental  structure  of  the  work  in  [1]  to  the  full  3-dimensional 
case,  in  which  the  position  triplet  has  x,  y,  and  z  directions  and  the  control  inputs  are 
heading  (T)  and  flight  path  (rypath)  angles.  Within  this  framework,  the  system  description 
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of  Equation  (2.21)  becomes 


x(f) 

=  scos(^(t))cos(7pat/l(f)) 

y(t) 

=  ssin(^(t))cos(7pa^(t)) 

z(t) 

=  ssin(7  path(t)) 

m 

=  Ul(t) 

If  path (0 

=  U2{t) 

(3.25) 


and  the  maximum  control  input  inequality  of  Equation  (2.22)  is  expanded  to 

1^1, max 

^2, max  (3.26) 

However,  this  set  of  constraints  on  the  control  input  is  incomplete.  In  any  realizable  air 
vehicle,  there  exists  a  necessary  trade-off  between  turning  and  climbing  performance.  For 
instance,  this  relationship  may  be  expressed  in  terms  of  a  maximum  centripetal  acceler¬ 
ation,  such  that  the  application  of  both  control  inputs  may  not  exceed  a  certain  g- load 
threshold.  Without  presenting  a  form  for  the  exact  relationship,  we  are  assured  of  the 
fact  that  one  cannot  blindly  attempt  to  apply  the  maximum  control  input  on  both  input 
channels.  Keeping  in  mind  that  we  are  operating  with  an  incomplete  set  of  control  input 
constraints,  we  will  continue  this  3-dimensional  extension  until  the  need  for  additional 
system  characterization  is  more  apparent. 

Our  next  step  is  to  determine  the  3-dimensional  version  of  the  system  Hamiltonian 

H(x(t),u(t),  70,7(*))  =  7o  +  [7iW«cos(^(t))  +72(t)ssin(^(t))]cos(7patft(f)) 

+  73(i)ssin(7Pat/lW)  +74(*)«i(0  +  75(^2^)  (3.27) 

and  then  begin  to  apply  the  conditions  of  Pontryagin’s  minimum  principle  from  Equation 
(2.18)  and  the  auxiliary  inequality  condition  implied  by  Equation  (2.20).  Avoiding  the 
exercise  in  simple  mathematics,  we  concentrate  on  the  inequality  shown  below  since  it 


“ill  < 
M  < 
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provides  an  excellent  indication  of  insufficient  system  information 


VwG/i  (3.28) 

After  inserting  the  Hamiltonian  of  Equation  (3.27),  using  the  same  cost  function  as  in 
Equation  (2.23),  and  simplifying  the  expressions,  we  arrive  at  the  following  equation 

7 4 (*)“*(*)  +  lS)u*2{t)  <  lS)ui(t)  +  7 5(t)u2{t),  Vu  €  n  (3.29) 

Here  is  a  prime  example  of  the  need  for  a  manner  to  express  the  trade-off  between  the  two 
control  inputs.  Unlike  the  results  from  Equation  (2.25)  and  Equation  (2.26),  the  relation 
above  does  not  have  a  simple  specific  solution. 

At  this  point,  the  3-dimensional  extension  of  the  CLC  control  technique  must  be 
deferred  until  an  adequate  characterization  of  the  interplay  between  heading  rate  and 
climb  rate  is  available  and  a  recommendation  will  be  presented  in  Chapter  5. 

3.5.2  Model  Predictive  Control  Commanding.  Conceptually,  the  extension  of 
model  predictive  control  techniques  to  a  3-dimensional  approach  is  quite  easy.  The  form  of 
the  non-linear  system  model  of  Equation  (2.14)  and  the  MPC  cost  summation  of  Equation 
(2.31)  is  unaltered.  We  are  able  to  apply  the  same  general  procedure,  as  outlined  for  2- 
dimensional  problems  in  [9] ,  to  determine  the  optimal  perturbation  control  input.  However, 
the  complexity  of  the  problem  is  greatly  increased  by  the  addition  of  a  third  direction 
of  motion.  In  order  to  complete  this  3-dimension  extension  of  the  MPC  scheme,  two 
modifications  to  the  problem  description  are  required. 

The  first  change  that  must  be  made  is  to  define  the  six-degree-of-freedom  flight  model 
in  the  form  of  the  general  non-linear  system  of  Equation  (2.14).  Let  us  assume  the  state 
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and  control  input  vectors  are  defined  for  a  2-dimensional  world  as 


V 

Pnorth 
Peast  ‘ 

'•P 

P'rate 


U  = 


throttle 

rdrcmd 


(3.30) 


where  V  is  the  magnitude  of  the  vehicle’s  velocity  vector,  pnorth  and  Peast  are  the  northward 
and  eastward  positions  of  the  vehicle,  ip  is  the  yaw  angle,  and  iprate  is  the  rate  of  change 
of  the  yaw  angle.  Note  that  the  throttle  input  is  a  percentage  of  the  maximum  throttle 
setting  and  the  rdrcmd  input  is  the  commanded  rudder  deflection  angle.  In  order  to  extend 
this  problem  to  a  3-dimensional  world,  the  state  vector  must  be  augmented  to  incorporate 
the  additional  states  which  account  for  the  linear  and  rotational  kinematics  in  the  new 
degrees  of  freedom,  and  the  control  input  vector  must  be  expanded  to  include  commands 
for  the  remaining  control  surfaces.  This  augmentation  process  modifies  the  state  and 
control  input  vectors  to  their  generic  3-dimensional  definitions 


V 


Pnorth 

Peast 

h 


x  = 


<P 

e 


ip 

1 Prate 

Orate 


Ip- 


rate 


U  = 


throttle 
^  d-rnul 
d&cmd 
r  dr  cmd, 


(3.31) 


where  h  is  the  vehicle’s  altitude,  (p  is  the  roll  angle,  6  is  the  pitch  angle,  and  (prate  and 
0rate  are  the  rates  of  change  of  the  roll  and  pitch  angles,  respectively.  Similar  to  the 
control  input  vector  of  Equation  (3.30),  ailcmd  is  the  commanded  aileron  deflection  angle 
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and  elecmd  is  the  commanded  elevator  deflection  angle.  The  states  defined  in  Equation 
(3.31)  are  not  the  full  set  of  states  that  would  typically  be  included  for  a  realistic  model 
(additional  states  may  be  required  based  on  the  specific  controller  design),  but  this  state 
vector  definition  illustrates  the  simple  fact  that  the  expansion  to  a  fully  3-dimensional 
scenario  entails  a  significant  increase  in  the  amount  of  data  that  must  be  handled.  This 
increased  complexity  propagates  itself  throughout  the  MPC  structure  and  generates  a  much 
more  computationally  burdensome  optimization  problem. 

The  secondary,  yet  equally  critical  alteration  that  must  occur  is  to  expand  the  refer¬ 
ence  trajectory  to  incorporate  all  three  dimensions  of  the  vehicle’s  position.  Although  both 
changes  are  necessary,  this  modification  is  what  really  provides  the  capability  of  MPC  to 
be  exploited  for  3-dimensional  control  optimization  problems.  The  waypoints  that  define 
the  guidance  path  chosen  by  Algorithm  6  during  the  path  selection  operation  in  Figure 
3.1  provide  the  endpoints  for  each  segment  of  the  reference  trajectory,  and  one  must  then 
interpolate  between  the  points  to  determine  an  appropriate  reference  trajectory.  One 
obvious  interpolation  technique  is  simply  to  project  the  line-of-sight  between  successive 
waypoints.  Given  the  method  used  to  generate  the  set  of  waypoints,  this  LOS  projection 
guarantees  that  the  reference  trajectory  will  not  pass  through  any  of  the  obstacles. 

Despite  our  best  efforts,  a  properly  functioning  version  of  the  MPC-based  optimiza¬ 
tion  algorithm  was  not  achieved.  Nevertheless,  we  were  able  to  gain  some  insight  into 
possible  avenues  of  investigation  for  this  control  generation  approach.  These  recommen¬ 
dations  will  be  detailed  in  Chapter  5. 

3.5.3  Minimum  Effort  Control  Commanding.  In  the  typical  minimum  effort 
control  setting,  the  controllability  Grammian  is  used  to  determine  the  full  set  of  inputs 
which  will  affect  the  desired  state  change  while  requiring  the  least  amount  of  energy  at 
the  input.  Note  that  this  is  not  a  perturbation  control  method.  These  control  inputs  are 
specified  by  Equation  (2.47),  which  is  reprinted  below 

u*(t )  =  BT&T(tf  —  t)Wfl{tf)x,  0  <  t  <  tf 
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Although  this  basic  control  optimization  technique  may  be  used  to  guide  a  system  to  a 
specific  set  of  points  in  state-space,  there  is  no  method  to  predetermine  the  path  that  the 
state  trajectory  will  follow  in  between  the  state  vector  setpoints.  The  state  trajectory 
resulting  from  the  application  of  a  control  input  generated  by  minimum  effort  control 
methods  may  "wander"  through  state-space  before  reaching  the  terminal  point.  Given 
that  we  desire  to  avoid  obstacles,  a  specific  "safe"  path  must  be  followed  through  state- 
space  and  a  modified  view  of  minimum  effort  control  is  necessary  in  order  to  make  this 
technique  a  viable  option. 

This  "safe"  trajectory  has  already  been  determined,  as  the  line-of-sight  path  between 
waypoints,  by  the  waypoint  generation  and  path  selection  processes.  Furthermore,  the 
optimal  set  of  waypoints  was  selected  by  using  the  controllability  Grammian  in  a  manner 
based  on  minimum  effort  control.  Recalling  that  one  of  the  initial  assumptions  of  this 
research  is  that  heading,  altitude,  and  velocity  hold  autopilots  are  available,  the  information 
explicitly  contained  within  the  chosen  set  of  waypoints  may  be  used  to  generate  the  control 
inputs  directly.  In  order  to  travel  between  two  consecutive  waypoints,  wp  ,  and 
WP next ’  sPecific  inputs  applied  to  the  system  are  the  desired  speed,  the  altitude  of 
wp  , ,  and  the  horizontal  plane  heading  angle  of  the  vector  from  wp  ,  to  wp 

— —next 7  ±  o  o  — f_ current  — —next 

Finally,  to  ensure  that  the  line-of-sight  path  is  closely  followed,  the  control  inputs  must  be 
generated  at  a  sufficiently  high  enough  rate.  Bearing  this  in  mind,  the  series  of  inputs  are 
computed  such  that  all  turns  and  altitude  changes  are  accomplished  in  a  smooth  manner 
and  the  line-of-sight  path  is  closely  followed. 

3.6  Flight  Simulator 

Based  on  our  assumptions  and  the  data  available  from  the  command  generator,  a  few 
modifications  to  the  basic  simulation  model  of  Figure  2.5  are  necessary.  The  first  change  is 
merely  the  removal  of  the  linear  (lin_accel)  and  rotational  (rot_accel)  acceleration  terms. 
Recall  that,  in  the  original  project  for  which  this  model  was  used  [11],  these  linear  and 
rotational  acceleration  terms  were  included  in  an  effort  to  evaluate  the  forces  sensed  by  a 
pilot  in  various  flight  conditions  and  were  not  used  to  modify  the  dynamics  of  the  overall 
system.  The  omission  of  these  six  terms  is  possible  since  we  are  not  interested  in  this 
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Figure  3.5  Modified  simulation  model 

information  and  the  loss  of  this  data  does  not  degrade  the  performance  of  the  rest  of  the 
system.  Another  simple  change  is  the  addition  of  an  exit  condition  to  halt  the  simulation 
prematurely  when  the  munition  reaches  the  intended  target. 

The  more  significant  alterations  are  in  the  specific  input  we  apply  to  the  system  and 
the  details  of  the  feedback  controllers.  In  general,  the  input  we  apply  is  determined  by 
the  control  optimization  method  we  choose  to  employ,  and  this  choice  will  force  changes  to 
the  feedback  controller  structure.  Since  the  modified  minimum  effort  control  technique  is 
the  only  command  generation  method  being  implemented  in  three  dimensions,  the  control 
input  takes  the  form  of  a  commanded  speed,  altitude,  and  heading.  While  the  controllers 
are  still  autopilots  designed  to  hold  the  vehicle  in  a  given  configuration,  we  have  redefined 
the  reference  values  to  be  the  desired  speed,  altitude  and  heading,  instead  of  the  equilibrium 
conditions.  This  change  is  required  since  the  original  model  of  Figure  2.5  was  designed 
to  track  the  equilibrium  speed,  altitude,  and  heading  values  and  external  inputs  were 
available  to  apply  changes  directly  to  the  actuator  deflection  angles.  For  simplicity,  these 
autopilots  are  designed  as  simple  proportional  feedback  controllers  which  have  constant 
gains  determined  in  an  ad  hoc  manner  and  provide  an  adequate  level  of  responsiveness. 
The  new  simulation  model  is  shown  in  Figure  3.5. 
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3. 7  Summary 

In  this  chapter,  we  have  presented  the  various  innovations  of  this  thesis.  Section  3.2 
detailed  a  simple,  intuitive  method  to  assess  the  value  of  a  given  target  within  a  multi-target 
scenario.  Next,  Section  3.3  developed  the  procedure  to  determine  potential  waypoints 
for  the  purpose  of  obstacle  avoidance.  Section  3.4  then  employed  minimum  effort  control 
concepts  to  derive  a  method  to  select  a  desired  path  from  the  candidate  trajectories  defined 
during  the  waypoint  generation  process.  The  extension  of  CLC,  MPC,  and  minimum  effort 
control  techniques  to  three  dimensions  was  presented  in  Section  3.5.  However,  functioning 
versions  of  the  CLC  and  MPC  methods  were  not  achieved.  Finally,  Section  3.6  detailed 
the  adaptation  of  the  simulation  model  to  account  for  the  unique  elements  of  this  research. 
Given  the  algorithms  and  simulation  framework  developed  in  this  chapter,  we  are  ready 
to  discuss  the  test  cases  and  analyze  the  results  of  these  simulations. 
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IV.  Simulation  Implementation  and  Results 

4-1  Introduction 

Throughout  this  chapter,  we  present  several  test  cases  in  an  effort  to  highlight  both 
the  strengths  and  weaknesses  of  the  path  planning  and  control  optimization  procedures. 
In  general,  two  planned  paths  are  illustrated  for  each  scenario.  The  first  path,  resulting 
from  the  path  selection  algorithm  developed  in  Section  3.4  of  this  research,  is  known  as 
the  Minimum  Effort  Path  (MEP).  The  other  path  is  the  Minimum  Distance  Path  (MDP), 
which  is  obtained  by  selecting  the  path  with  the  lowest  cumulative  distance.  Once  a 
candidate  path  is  selected,  a  command  generator  function  is  used  to  compute  the  control 
inputs  and  thus  drive  the  vehicle  along  the  selected  path.  The  MPC  algorithm  was 
originally  chosen  to  perform  this  function,  but  due  to  implementation  issues  with  the 
MPC  approach,  a  simplified  command  generator  based  on  the  MEP  waypoints  is  used. 
As  such,  one  can  envision  the  problem  being  decomposed  into  navigation  and  control 
functions,  where  the  MEP  and  MDP  selection  processes  represent  navigation.  The  control 
methods  include  the  CLC,  MPC,  and  MEP-based  command  generator  techniques.  As 
stated  in  Chapter  3,  the  extremal  control  (CLC)  and  model  predictive  control  optimization 
approaches  are  not  used  in  these  simulations,  but  we  will  discuss  the  implementation  issues 
encountered  when  using  the  MPC  command  generator. 

(K) 

We  start  the  analysis  with  a  description  of  the  MATLAE>  implementation  of  the 
MPC  and  MEP-based  command  generator  techniques.  Following  the  discussion  of  imple¬ 
mentation  issues,  two  basic  scenarios  are  presented  to  demonstrate  the  simulation  environ¬ 
ment.  Next,  scenarios  of  the  first  test  set  are  designed  to  highlight  the  characteristics  of 
the  path  planning  algorithm.  Finally,  the  simulations  of  the  second  test  set  are  devoted 
to  the  characteristics  of  the  MEP-based  control  technique.  Note  that,  for  now,  we  assume 
the  targets  are  stationary  and  the  target  tracking  aspect  is  bypassed. 
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4-2  Command  Generators 

As  mentioned  above,  efforts  were  made  to  implement  the  MPC  and  MEP-based  com- 
rnand  generators  in  MATLABW.  The  following  is  a  presentation  of  the  implementation 
details  and  a  discussion  of  the  issues  which  arose  from  each  command  generation  technique. 

4.2.1  Model  Predictive  Control  Command  Generator.  As  stated  at  the  end  of 
Section  3.5.2,  a  functioning  version  of  the  3-dimensional  MPC  optimization  technique  was 
not  achieved.  However,  the  important  aspects  of  the  implementation  of  this  approach 

(r) 

in  MATLAE>  must  be  noted  in  order  to  assist  any  future  efforts  related  to  this  topic. 
Recalling  that  the  general  form  of  this  method  in  three  dimensions  is  identical  to  the  form 
of  the  2-dimensional  problem,  we  revisit  the  presentation  of  the  MPC  approach  in  Section 
2.3.2.  Specifically,  we  focus  on  Equation  (2.33),  which  is  reproduced  below 

Jk  =  r,T(YTTY  +  UTTU)r,  +  2[{y_nom  -  r)TYY  +  u^TUjr, 

and  explain  the  method  to  obtain  the  U  and  Y  matrices. 

First,  the  simpler  of  the  two  matrices,  U,  is  formed  for  a  single  input  by  selecting  a 
set  of  basis  functions.  The  basis  functions  are  then  evaluated  at  N  discrete-time  sample 
points,  where  N  is  the  length  of  the  finite  horizon.  If  Tk  is  the  continuous-time  interval 
for  the  finite  horizon  and  Ts  is  the  sample  interval,  then  the  exact  discrete-time  interval 
is  computed  as  JV  =  -Y- .  Now,  for  each  basis  function,  we  obtain  a  vector  of  N  values 
and,  given  that  we  intend  to  implement  this  technique  with  several  basis  functions  and 
multiple  inputs,  we  orient  the  vector  so  that  the  dimensions  are  1  x  1  x  N.  For  a  set  of 
nbasis  basis  functions,  the  U  matrix  is  formed  such  that  the  individual  vectors  constitute 
the  columns  of  the  matrix  and  the  dimension  of  U  is  1  x  n^asis  x  AT.  When  we  extend  this 
to  the  general  case  of  m  inputs,  the  final  U  matrix  is  generated  by  placing  identical  copies 
of  the  single-input  form  of  U  on  the  main  diagonal  of  a  block  diagonal  matrix  and  the 
dimensions  of  this  final  matrix  are  m  x  ( m*nbasis )  x  N.  As  an  example,  for  the  four  input 
system  defined  by  Equation  (3.31)  and  a  set  of  five  Laguerre  polynomial  basis  functions, 


4-2 


the  single-input  version  of  the  U  matrix  is  given  by 


C/i 


£0  £l  £2  £3  £4 


(4.1) 


where  e*  is  computed  using  [16] 


£»(&) 


dtl 


i  =  0:4,  k  =  0  :  (JV  -  1) 


(4.2) 


Note  that  these  £  functions  are  determined  by  the  choice  of  the  basis  functions.  Finally, 
the  multi-input  version  is  formulated  as 
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where  0iX5  is  a  row  vector  of  five  zeros  and  the  full  dimension  of  U  is  4  x  20  x  N. 

Next,  we  consider  the  more  complex  generation  of  the  output  relationship  matrix, 
Y .  In  Chapter  2  we  mentioned  that  Y  is  formed  by  exciting  the  nonlinear  system  with  a 
series  of  test  inputs  on  a  channel  by  channel  basis.  Now  we  explain  this  process  in  greater 
detail,  starting  with  the  formulation  of  the  input  to  be  applied.  Since  we  are  working  with 
a  set  of  basis  functions  to  represent  the  perturbations  about  some  nominal  input,  we  must 
determine  the  system  response  to  each  basis  function  as  subdivisions  of  the  response  due 
to  input  channelization.  In  order  to  accomplish  this  task,  we  form  the  perturbed  input  as 


uli(k)  =  ul0{k)  +  U  (k)i_ 7.  (4.4) 

where  u\{k)  is  the  Ith  input  perturbed  by  the  ith  basis  function  at  time  sample  k,  ul0(k)  is 
the  Ith  nominal  input  at  time  sample  k,  U{k)  is  the  U  matrix  at  time  sample  k ,  and  q.  is 
used  to  isolate  the  ith  basis  function.  For  example,  to  isolate  the  third  function  in  a  set 
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of  five  basis  functions,  we  use 


—3 


0  0  1 


T 


0  0 


(4.5) 


The  nominal  input  used  throughout  this  research  is  the  equilibrium  input  values  required 
for  the  munition  to  maintain  a  given  set  of  trim  conditions  (speed,  heading,  and  flight-path 
angle).  In  order  to  determine  the  system  response  to  the  perturbational  component  of  the 
input  over  the  finite  horizon,  we  must  first  excite  the  nonlinear  system  with  the  perturbed 
input  and  then  subtract  off  the  nominal  system  response.  The  nominal  system  response 
is  obtained  by  driving  the  nonlinear  system  with  the  nominal  input.  For  the  case  of  four 
inputs  and  five  basis  functions,  the  nominal  output,  yo,  and  the  perturbed  response,  y\, 
are  computed  using  the  following  inputs: 


uo 

uo 


«o  uo 

ul  uo 


u0 

u0 


1 T 

yo 

(4.6) 

i  T 

= 

=>  yi 

(4.7) 

where  we  have  used  the  response  to  the  second  input  perturbed  by  the  third  basis  function 
as  an  example  for  y\.  Next,  we  generate  the  perturbational  component  of  the  response  by 


5i  =  yi-yo 


(4.8) 


and,  given  the  assumption  that  5y  =  Yr),  we  may  finally  form  the  output  relationship 
matrix.  Similar  to  the  U  matrix,  the  Y  matrix  involves  a  peculiar  dimensionality,  such  that 
the  dimensions  of  the  output  relationship  matrix  for  a  system  with  n  outputs,  m  inputs, 
ribasis  basis  functions,  and  a  discrete-time  horizon  of  length  N  are  n  x  (m  *  ribasis )  x  N. 
The  column  vector  Syl,  of  dimension  n  x  1  x  N,  then  becomes  column  (l  —  1)  *  ribasis  +  * 
in  the  Y  matrix. 

Now  that  we  have  generated  the  U  and  Y  matrices,  we  are  ready  to  address  the 

optimization  routine.  The  minimization  of  the  cost  defined  by  Equation  (2.33)  is  handled 

(R) 

by  quadprog ,  a  quadratic  programming  function  available  in  MATLAB  ,  which  solves 
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constrained  quadratic  minimization  problems  of  the  form  given  below 

min(0.5  *  g?Ax  +  y^x),  such  that  Ax  <  b  (4.9) 

X 

However,  given  the  unique  dimensions  of  the  U  and  Y  matrices,  a  different  manner  of 
expressing  the  cost  function  is  necessary.  We  first  rewrite  the  cost  function  of  Equation 
(2.33)  in  the  proper  summation  notation  as 

k+N—l 

Jk  =  Y,  {rLT[YT(i)TYW  +  UT(i)rU{i)\i1  +  2[{y0(i)  -r(i))TTY(i)  +1g(i)TU(i)]rL} 

i=k 

(4.10) 

where  the  notation  Y  (i)  refers  to  the  value  of  the  Y  matrix  at  time  sample  i  such  that 
Y{i)  =  Y(:,  :,i),  with  similar  definitions  for  the  U  matrix  and  ,  r,  and  u0  vectors.  In 
order  to  cast  this  cost  function  in  the  form  of  Equation  (4.9),  we  note  that  the  summation 
may  be  collapsed  into  a  single  expression  by  summing  the  appropriate  terms  over  their 
time  dimension  such  that  we  obtain 

k+N-l 

Ak  =  [YT(i)TY(i)  +  UT(i)rU(i)\  (4.11) 

i—k 

k+N- 1 

Yk  =  [(MoW  -r{i))TTY{i)  +u£ (i)ru(i)}  (4.12) 

i—k 

Jk  =  rnT  Ak'n  +  ‘2yl/ri  (4.13) 

The  cost  function,  Jk,  is  now  in  the  format  required  by  quadprog. 

However,  when  we  attempt  to  implement  this  command  generation  technique  in 
MATLAB^  software,  we  encounter  a  major  problem.  The  perturbation  input,  obtained 

by 

Su(i )  =  U(i)rj  (4.14) 

where  Su(i)  is  the  perturbational  input  at  time  sample  i  and  U(i )  is  the  value  of  the 
U  matrix  at  time  sample  i.  tends  to  generate  illogical  control  inputs.  For  example, 
most  of  the  test  cases  used  during  the  development  of  this  command  generator  resulted  in 
perturbational  inputs  for  the  aileron,  elevator,  and  rudder  deflection  angles  on  the  order  of 
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±3600°.  Clearly  an  input  of  this  magnitude  is  not  a  rational  option  to  apply  to  a  control 
surface  which  has  physical  deflection  angle  limits  on  the  order  of  ±30°.  Nevertheless,  the 
simulation  attempts  to  apply  these  control  inputs  and,  since  the  script-file  which  represents 
the  air  vehicle  limits  the  control  inputs  based  on  internal  saturation  values,  a  system 
response  is  obtained  for  a  short  period  of  time  until  the  operation  of  the  script-file  fails. 
The  irrational  input  values  appear  to  be  caused  by  an  error  in  the  method  used  to  compute 
the  A  matrix  and  the  vector.  Once  this  computational  method  was  corrected,  the 
perturbational  inputs  took  on  more  realistic  values  and  the  simulation  would  persist  for 
a  longer  period  of  time.  Yet,  even  when  we  attempt  to  fly  the  munition  at  a  constant 
speed  in  a  steady-level  configuration,  the  script-file  which  represents  the  vehicle’s  dynamics 
numerically  continues  to  fail  in  a  similar  manner.  Given  that  previous  research  efforts 
[9,  15,  16]  have  achieved  successful  MPC  optimization  routines  in  two  dimensions,  this 
problem  appears  to  be  caused  by  the  details  of  our  MATLABW  implementation  of  the 
six-degree-of-freedom  dynamics  model. 

4-2.2  MEP-based  Command  Generator.  In  Section  3.5.3  we  presented  a  modified 

minimum  effort  control  scheme  that  was  based  on  the  information  provided  by  the  way- 

points  which  constitute  the  minimum  effort  path.  This  MEP-based  command  generation 

process  is  decomposed  into  two  separate  subfunctions.  One  routine  determines  when  the 

current  waypoint  has  been  reached  and  then  commands  the  munition  to  fly  to  the  next 

waypoint  along  the  pre-planned  path.  The  other  subfunction  generates  the  actual  control 

inputs  in  a  manner  which  produces  smooth  command  sequences.  The  following  discus- 

(r) 

sion  presents  the  details  of  the  MATLABW  implementation  of  the  MEP-based  command 
generator. 

First,  before  any  control  inputs  are  computed,  we  must  formulate  a  method  to  declare 
a  waypoint  has  been  reached.  Given  the  near  certainty  that  the  actual  flight  path  will 
not  pass  directly  through  the  waypoint,  we  are  forced  to  make  this  declaration  based  on 
the  munition’s  proximity  to  the  current  waypoint.  One  simple  solution  to  this  problem  is 
to  make  the  decision  to  fly  to  the  next  waypoint  when  the  actual  trajectory  passes  within 
a  specific  distance  of  the  current  waypoint.  However,  there  is  still  no  guarantee  that 
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the  munition’s  trajectory  will  satisfy  this  proximity  criterion.  A  more  robust  solution  to 
this  issue  was  achieved  by  monitoring  the  trend  of  the  distance  between  the  munition  and 
the  current  waypoint.  Since  we  expect  this  distance  to  be  a  monotonically  decreasing 
function  of  time,  the  decision  to  proceed  to  the  next  waypoint  is  made  when  the  distance 
at  the  current  time  sample  is  greater  than  the  distance  at  the  previous  time  sample.  In 
order  to  provide  an  extra  level  of  confidence,  this  decision  may  by  easily  delayed  until  the 
increasing  distance  phenomenon  is  observed  over  a  few  consecutive  time  epochs.  Once 
the  decision  is  made,  due  to  the  nonlinear  nature  of  this  six-degree-of-freedom  model,  we 
must  recompute  the  trim  conditions  for  the  air  vehicle  to  fly  at  the  new  heading  angle 
and  flight-path  angle,  as  defined  by  the  vector  from  the  munition’s  current  position  to  the 
redefined  current  waypoint. 

As  stated  in  Section  3.5.3,  the  control  inputs  are  obtained  directly  from  the  position 
data  which  define  the  current  and  previous  waypoints.  Furthermore,  we  compute  the 
control  inputs  such  that  we  avoid  large-magnitude  step  changes  at  the  inputs.  These 
smooth-transitioning  command  sequences  are  necessary,  since  the  simulation  has  a  ten¬ 
dency  to  crash  when  a  large-magnitude  change  to  the  input  is  commanded  within  a  single 
time  sample.  Therefore,  a  simple  command  generation  technique  was  used  to  generate 
the  following  input  vector: 

(4.15) 

where  Vcmd  is  the  desired  constant  speed,  hcmcj  is  the  desired  altitude,  and  4 'cmd  is  the 
desired  heading  angle.  In  order  to  smooth  out  the  hcmd  input,  we  use  the  munition’s 
altitude  at  the  time  when  the  previous  waypoint  was  reached  as  the  starting  point  for 
a  straight-line  segment  which  terminates  at  the  altitude  of  the  current  waypoint.  The 
commands  at  each  time  sample  for  this  straight-line  segment  are  generated  as  a  simple  time 
propagation  at  the  desired  speed  along  the  flight-path  angle  determined  by  a  LOS  vector 
between  the  previous  and  current  waypoints.  The  exact  magnitude  of  the  incremental 
change  in  altitude  is  determined  by  the  sample  interval,  vehicle  speed,  and  flight-path 


u  = 


Vcmd 

hcmd 

4* cmd 
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Table  4.1  Starting  location  and  target  position  for  basic  demonstration 


North  (ft) 

East  (ft) 

Altitude  (ft) 

Munition  Start 

0 

0 

1000 

Target  Location 

20000 

0 

0 

angle 

A/lcmd  =  TsVcmd  Sin  ('/path)  (4-16) 

For  the  transitions  on  the  T cmd  input,  we  employ  a  static  turn  rate  value,  chosen  in  an 
ad  hoc  manner,  to  compute  small-magnitude  incremental  changes  to  the  input  at  each 
time  sample.  For  example,  in  a  discrete-time  simulation  with  a  time  step  of  0.01  s  and  a 
static  turn  rate  of  30  degrees  per  second,  the  MEP-based  command  generator  computes 
incremental  heading  changes  of,  at  most,  0.3°  at  each  time  sample.  We  are  now  ready 
to  present  the  simulation  scenarios,  starting  with  a  basic  demonstration  of  the  simulation 
environment. 

4-3  Basic  Demonstration 

Before  we  address  the  various  test  cases,  we  first  demonstrate  the  simulation  envi¬ 
ronment  through  two  basic  scenarios  in  which  we  can  predict  the  selected  path.  In  these 
two  examples,  we  place  obstacles  in  a  manner  that  forces  the  minimum-distance  path  and 
the  minimum-effort  path  to  be  identical.  These  examples  will  also  serve  the  purpose  of 
demonstrating  the  typical  situations  in  which  other  researchers  [9]  have  used  model  pre¬ 
dictive  control  in  two  dimensions.  Even  though  we  have  not  been  able  to  replicate  this 
MPC  command  generation  technique,  we  have  at  least  formulated  a  method  to  produce 
the  waypoints  and  reference  trajectories  required  by  the  MPC  approach.  For  simplicity, 
both  examples  will  use  the  same  initial  and  termination  positions  as  given  in  Table  4.1. 

In  the  first  situation,  we  construct  the  scene  by  placing  a  short,  wide  obstacle  directly 
in  between  the  munition  and  the  target  in  order  to  force  the  path  planner  to  choose  the 
up-and-over  path.  This  simple  scene  is  shown  with  all  the  candidate  paths  in  Figure  4.1 
where  the  red  path  is  the  chosen  MDP/MEP  candidate,  and  the  blue  paths  are  the  other 
non-selected  candidate  paths.  Given  the  definition  of  the  path  selection  cost  function  in 
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Table  4.2  Path  selection  cost  values  for  scenario  in  which  trajectory  is  forced  up-and- 
over  the  obstacle  -  Path  1  is  up-and-over,  Paths  2  and  3  are  around,  Paths  4 
and  5  are  up-and-around 


Path  #1 

Path  #2 

Path  #3 

Path  #4 

Path  #5 

Orientation 

Travel 

Distance 

0.208203 

0.200385 

0.07008 

0.00143559 

0.198368 

0.370164 

0.001436 

0.198368 

0.370164 

0.394462 

0.201438 

0.094796 

0.394462 

0.201438 

0.094796 

Total  Cost 

0.478668 

0.569968 

0.569968 

0.690696 

0.690696 

Figure  4.1  Scenario  in  which  trajectory  was  forced  up-and-over  the  obstacle  -  red  path 
is  MEP/MDP  and  blue  paths  are  non-selected  candidates 

Equations  (3.6)  and  (3.22),  we  expect  the  around  paths  to  have  large  values  for  J distance  and 
J travel ■  Furthermore,  knowing  that  the  up-and-over  path  will  be  the  minimum-distance 
path,  we  anticipate  this  path  will  have  the  smallest  value  for  Jdistance ■  The  individual 
cost  values  for  each  path  are  shown  in  Table  4.2  and  the  trend  for  Jdistance  matches  our 
expectation. 

The  second  example  is  generated  by  using  a  tall,  thin  obstacle  in  an  effort  to  force 
the  path  planner  to  choose  the  around  path.  This  scenario  is  depicted  in  Figure  4.2  where 
the  red  path  is  the  chosen  MDP/MEP  candidate,  the  magenta  paths  are  candidate  paths 
with  unstable  systems,  and  the  blue  paths  are  the  remaining  non-selected  candidate  paths. 
A  discussion  of  these  unstable  paths  is  presented  in  Section  4.4.2.  Similar  to  the  previous 
example,  we  expect  the  around  paths  to  have  the  minimum  value  for  Jdistance ■  Since  the 
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Figure  4.2  Scenario  where  the  selection  of  the  around  path  is  forced  -  red  path  is 
MEP/MDP,  magenta  paths  are  unstable/infeasible,  and  blue  path  is  an  un¬ 
selected  candidate 

up-and-over  and  up-and-around  paths  produce  unstable  systems  during  the  initial  climb 
to  the  top  of  the  obstacle,  these  three  candidate  paths  must  be  ignored  and  are  given 
component  and  total  costs  equal  to  zero.  Table  4.3  shows  the  component  and  total  costs 
for  each  candidate  path  in  this  example. 

4-4  Test.  Set  1:  Path  Planning  Characteristics 

4-4-1  Computational  Complexity.  One  of  the  major  shortcomings  of  this  mini¬ 
mum  effort  control  based  approach  is  the  computational  complexity  of  the  path  planning 
algorithm.  During  the  development  of  the  waypoint  generation  procedure  in  Section  3.3, 
it  was  observed  that  the  maximum  number  of  potential  paths  grows  exponentially  based 
on  the  number  of  obstacles  blocking  the  LOS  path  from  the  munition  to  the  target.  Figure 
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Table  4.3  Path  selection  cost  values  for  scenario  in  which  trajectory  is  forced  around  the 
obstacle  -  Path  1  is  up-and-over,  Paths  2  and  3  are  around,  Paths  4  and  5  are 
up-and-around 


Path  #1 

Path  #2 

Path  #3 

Path  #4 

Path  #5 

Orientation 

0 

0.499999 

0.500001 

0 

0 

Travel 

0 

0.5 

0.5 

0 

0 

Distance 

0 

0.5 

0.5 

0 

0 

Total  Cost 

0 

1.499999 

1.500001 

0 

0 

Table  4.4  Path  comp’ 

lexity  data 

t"wp_gen 

tpath  sel 

Tlpaths 

Single- Obstacle  Case 

0.32  s 

31.586s 

5 

Two-Obstacle  Case 

5.138s 

235.944  s 

25 

Three-Obstacle  Case 

19.328  s 

1622.724  s 

117 

4.3  depicts  the  evolution  of  candidate  paths  when  the  munition  must  avoid  an  increasing 
number  of  obstacles. 

In  Table  4.4,  we  can  clearly  see  the  rapid  growth  in  the  number  of  candidate  paths, 
fipaths >  that  are  passed  to  the  path  selection  routine.  Another  indication  of  the  compu¬ 
tational  complexity  inherent  to  this  path  planning  algorithm  is  the  amount  of  time  spent 
generating  waypoints,  twp  gen ,  and  selecting  the  minimum  effort  path,  tpath  Sei-  Since  the 
minimum  effort  path  planning  algorithm  used  in  these  simulations  was  not  implemented 
with  any  deliberate  attempt  at  efficiency,  the  exact  amount  of  time  required  by  these 
tasks  is  not  of  interest.  However,  the  key  point  to  notice  is  the  exponential  growth  in 
the  amount  of  time  necessary  for  the  waypoint  generation  and  path  selection  functions  to 
complete  their  tasks  as  shown  in  Figure  4.4.  Notice  that  the  number  of  candidate  paths 
grows  by  a  factor  of  5  for  each  obstacle  that  must  be  avoided,  while  the  associated  path 
selection  time  grows  by  a  factor  of  7.  Methods  to  mitigate  this  exponential  growth  are  a 
subject  of  future  research. 

4-4-2  Unstable  Paths.  The  primary  advantage  of  this  path  planning  technique  is 
the  built-in  capability  to  guide  the  munition  along  a  path  that  is  assured  to  be  feasible, 
in  terms  of  system  stability  and  controllability,  while  the  minimum  distance  approach  to 
path  planning  cannot  make  such  an  assurance.  There  is  no  guarantee  that  the  minimum 
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Target  Location 


Figure  4.3  Increasing  path  complexity  -  red  path  is  MEP  and  black  path  is  MDP 
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-♦ —  Path  Selection  — ■ —  Waypoint  Generation 


Number  of  Obstacles 


Figure  4.4  Trend  data  for  growth  of  waypoint  generation  and  path  selection  times 

distance  path  will  lead  to  a  stable  system  throughout  the  munition’s  flight.  One  such 
instance  is  created  by  modifying  the  scenario  depicted  in  Figure  4.3  and  moving  the  target’s 
position  closer  to  the  obstacle.  Graphically,  this  is  shown  in  Figure  4.5. 

In  order  to  demonstrate  the  unstable  system,  we  bypass  the  path  selection  function 
and  force  the  munition  to  traverse  the  minimum  distance  path.  The  result  of  flying  the 
munition  along  this  path  is  shown  in  Figure  4.6  and,  by  zooming  in  on  the  boxed  area, 
Figure  4.7  shows  that  the  system  quickly  becomes  unstable  after  reaching  the  waypoint 
on  the  far  side  of  the  obstacle.  One  of  the  main  benefits  of  the  minimum  effort  path 
planning  algorithm  is  exemplified  by  this  test  case.  The  MEP  algorithm  has  the  inherent 
ability  to  predict  the  emergence  of  system  instabilities  on  each  of  the  potential  paths 
and  then  ignore  these  unstable  paths.  This  prediction  capability  is  realized  during  the 
controllability  Grammian  computation  of  the  path  selection  process,  wherein  we  compute 
a  linearized  model  of  the  closed-loop  system.  In  order  to  anticipate  an  unstable  system,  we 
investigate  the  eigenvalues  of  the  closed-loop  system  dynamics  matrix  and,  using  classical 
stability  definitions  [14],  declare  the  system  to  be  unstable  if  any  of  the  eigenvalues  are 
greater  than  zero. 
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5000 


Target  Location 


Munition  Start 


5000 


5000 


East  Position  (ft) 


Figure  4.5 


Scenario  where  MDP  produces  unstable  system 


Target  Location 


Figure  4.6  Unstable  system  response  using  MDP 
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Figure  4.7  Zoomed  in  view  of  unstable  system  response  using  MDP 

4-5  Test  Set  2:  MEP-based  Control  Characteristics 

4-5.1  Target  Location  Relative  to  Obstacles.  One  situation  in  which  the  minimum 
effort  control  approach  to  input  generation  is  highly  effective  is  when  the  obstacles  and 
the  target  are  sufficiently  separated.  An  example  of  this  case  is  shown  in  Figure  4.8 
where  the  red  path  is  the  MEP,  the  black  path  is  the  MDP,  and  the  blue  path  is  the 
actual  flight  trajectory.  Notice  that  the  flight  trajectory  misses  all  of  the  obstacles  and 
terminates  at  the  target  (within  a  given  threshold  distance).  This  small  threshold  value 
is  necessary  since  the  target  has  its  own  size  characteristics  and  is  not  simply  a  point- 
mass  target.  Through  this  scenario,  we  see  that  the  minimum  effort  control  technique  is 
capable  of  generating  control  inputs  that  permit  the  munition  to  impact  the  target.  This 
is  only  possible  when  the  munition  has  enough  time  to  complete  the  heading  changes  after 
reaching  each  waypoint. 

For  the  case  of  a  target  located  near  an  obstacle,  the  minimum  effort  control  technique 
for  command  generation  performs  very  poorly.  An  example  of  this  case  is  shown  in  Figure 
4.9  where  the  red  path  is  the  MEP,  the  black  path  is  the  MDP,  and  the  blue  path  is  the 
actual  flight  trajectory.  Even  though  this  control  algorithm  does  not  drive  the  munition 
into  any  of  the  obstacles,  minimum  effort  control  fails  to  fly  the  munition  to  the  target 
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Target  Location 


5000 


3.5 


2.5  xIO 


0  5  North  Position  (ft) 


East  Position  (ft) 


Munition  Start 


Figure  4.8  Target  in  the  open 


since  there  is  not  enough  space  and  time  to  complete  the  turn  before  the  munition  passes 
the  target.  Note  that  in  this  case,  the  MDP  solution  is  not  a  viable  alternative  since  it 
would  produce  an  unstable  system  in  a  manner  similar  to  the  scenario  of  Section  4.4.2. 


4-5.2  Narrow  Alley  Scenario.  A  severe  downside  to  the  minimum  effort  control 
command  generation  method  is  that  it  does  not  force  the  munition  back  onto  the  LOS 
trajectory  after  a  heading  change  is  completed.  While  this  is  not  a  major  cause  for 
concern  in  situations  where  the  obstacles  are  spaced  far  apart,  as  the  separation  between 
obstacles  shrinks,  the  likelihood  of  the  munition  impacting  an  obstacle  during  and  after 
turns  continually  increases. 

An  example  of  this  narrow  alley  scenario  is  depicted  in  Figure  4.10  where  the  MEP 
solution  (red  path)  instructs  the  munition  to  travel  through  the  small  corridor  between 
two  obstacles.  In  order  to  better  illustrate  this  problem,  we  zoom  in  on  the  boxed  region. 
This  enlarged  view  is  presented  in  Figure  4.11  where  we  can  clearly  see  the  actual  flight 
trajectory  (the  blue  path)  passing  through  the  obstacle.  Again,  this  is  due  to  the  fact  that 
the  munition  cannot  execute  instantaneous  turns  and  minimum  effort  control  commanding 
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Altitude  (ft) 


Target  Location 


Figure  4.9  Target  located  close  to  obstacle 


Target  Location 


Figure  4.10  Narrow  alley  scenario 
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Figure  4.11  Zoomed  in  view  of  narrow  alley  scenario 

does  not  guide  the  munition  back  to  the  unobstructed  line-of-sight  path  between  waypoints. 
The  development  of  alternative  command  generators  is  a  subject  for  future  research. 

4-5.3  Target  Designation  Change  Scenario.  A  case  of  great  interest  for  the  multi¬ 
target  situation  is  a  demonstration  of  the  minimum  effort  control  method  in  response  to  a 
change  in  the  target  designation  during  the  munition’s  flight.  An  example  of  this  situation 
is  shown  in  Figure  4.12  where  the  red  path  is  the  final  MEP,  the  blue  path  is  the  actual 
flight  trajectory,  and  the  magenta  path  is  the  initial  MEP.  The  starting  location  for  the 
munition  and  both  target  positions  are  given  in  Table  4.5.  We  can  clearly  see  that  the  path 
planning  algorithm  accomplishes  its  task  of  generating  an  updated  MEP  in  response  to  the 
change  in  the  termination  point.  However,  the  control  input  generation  scheme  must  now 
adapt  to  this  newly  planned  path  and  fly  the  munition  toward  a  new  target  point.  The 
control  algorithm  is  just  as  capable  of  completing  this  task  as  it  would  be  if  the  scenario 
had  started  with  this  new  target.  We  still  need  to  be  concerned  with  the  potential  for 
impacting  an  obstacle  as  presented  in  the  narrow-alley  scenario  and  the  amount  of  space 
required  to  complete  a  turn  as  shown  in  the  case  of  a  target  located  close  to  an  obstacle. 
Overall,  the  minimum  effort  path  planning  algorithm  is  adaptable  to  target  reassignment. 
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Table  4.5 


Starting  location  for  munition  and  target  positions  for  target  designation 
change  scenario 


North  (ft) 

East  (ft) 

Altitude  (ft) 

Munition  Start 

0 

0 

1000 

Initial  Target 

30000 

0 

0 

Updated  Target 

24000 

-7500 

0 

Initial  Target  Location 


Figure  4.12  Target  designation  change  scenario 
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4-6  Summary 

In  this  chapter,  we  described  the  implementation  of  the  command  generators  in 

(K) 

MATLAE>  and  analyzed  the  results  of  various  test  scenarios.  Specifically,  Section  4.2 
detailed  the  implementation  of  the  MPC  and  MEP-based  command  generation  techniques 
and  discussed  the  major  issues  which  precluded  the  MPC  approach  from  being  employed 
in  the  simulations.  Next,  Section  4.3  presented  a  simple  demonstration  of  the  simulation 
environment  in  which  the  scene  was  constructed  such  that  the  munition  is  forced  to  select 
either  the  up-and-over  or  around  paths.  Next,  Section  4.4  investigated  the  characteris¬ 
tics  of  the  MEP  path  planning  algorithm,  highlighting  the  rapid  growth  in  computational 
complexity  and  the  inherent  ability  of  this  algorithm  to  predict  the  emergence  of  system 
instabilities.  Finally,  Section  4.5  tested  the  capability  of  the  MEP-based  command  gen¬ 
eration  approach  in  various  situations  of  interest,  to  include  the  case  of  a  large  change  to 
the  target’s  location  while  the  munition  is  in  mid-flight.  We  are  now  ready  to  conclude 
this  research  and  provide  recommendations  for  future  research  efforts  on  this  topic. 


4-20 


V.  Conclusions  and  Recommendations 


5.1  Research  Goal 

As  stated  in  Chapter  1,  the  goal  of  this  research  was  to  develop  a  path  planning  al¬ 
gorithm  that  provides  an  obstacle  avoidance  function  in  3-dimensional  space  and  to  inves¬ 
tigate  the  application  of  model  predictive  control,  circle-line-circle  control,  and  minimum 
effort  control  optimization  techniques  in  this  3-dimensional  setting.  Finally,  the  overall 
guidance  and  control  scheme  must  operate  in  an  environment  in  which  the  designated 
target  is  subject  to  change. 

5.2  Conclusions 

The  path  planning  algorithm  developed  by  this  research  was  segmented  into  two 
functions.  First,  the  waypoint  generation  routine  generates  sets  of  waypoints  that  define 
candidate  paths  in  a  manner  that  guarantees  obstacle  avoidance  along  the  line-of-sight  path 
between  successive  waypoints.  Next,  a  path  selection  procedure,  based  on  the  concept 
of  controllability  Grammians,  chose  the  candidate  path  which  requires  the  least  amount 
of  control  input  energy  to  traverse.  This  path  planning  method  has  proved  itself  to 
be  effective  at  producing  guidance  paths  that  are  free  of  obstructions.  Additionally, 
the  path  chosen  by  this  algorithm  is  also  guaranteed  to  be  feasible.  This  assurance 
is  possible  since  the  controllability  Grammian  approach  to  path  selection  facilitates  the 
identification  of  unstable  systems  and  the  detection  of  attempts  to  move  the  system  in 
an  uncontrollable  direction  of  the  state-space.  However,  there  is  a  large  overhead  cost  in 
terms  of  computational  time  required  by  this  method,  and  this  cost  increases  exponentially 
with  the  number  of  obstacles  that  must  be  avoided.  One  significant  shortcoming  of  this 
path  planning  algorithm,  though  it  appears  to  be  caused  by  the  implementation  method, 
is  that  it  has  a  strong  bias  towards  flying  at  lower  altitudes,  making  the  selection  of  the  up- 
and-over  path  a  very  rare  occurrence.  A  suggested  method  for  dealing  with  this  problem 
is  presented  in  Section  5.3.4. 
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In  Chapter  3,  we  attempted  to  extend  the  MPC  and  CLC  optimization  techniques 
to  3-dimensional  problems.  While  both  of  these  efforts  did  not  achieve  the  intended  level 
of  success,  we  have  gained  insight  into  how  the  extension  may  be  completed. 

For  the  model  predictive  control  optimization  scheme,  we  completed  the  theoretical 
extension  of  the  problem  in  [9, 15]  to  three  dimensions  but  encountered  numerous  failures 
during  the  implementation  of  this  command  generation  method.  Two  efforts  were  neces¬ 
sary  in  order  to  accomplish  the  extension  of  MPC  to  3-dimensions.  First,  the  dimensions 
of  the  matrices  and  vectors  required  by  the  quadratic  programming  optimization  technique 
had  to  be  greatly  increased  in  order  to  accommodate  the  larger  dimensions  of  the  air  ve¬ 
hicle’s  system  model.  As  a  complement  to  this  first  modification,  the  reference  trajectory 

had  to  be  specified  in  three  dimensions  in  order  to  take  full  advantage  of  this  optimization 

(R) 

method.  Unfortunately,  the  implementation  of  this  MPC  approach  in  MATLAB  ,  as 
shown  in  Section  4.2.1,  never  produced  meaningful  results.  Nevertheless,  once  we  achieve 
a  functioning  version  of  MPC  in  two  dimensions,  the  3-dimensional  implementation  will  be 
realized  by  simply  replacing  the  2-dimensional  system  with  the  flight  model  and  reference 
path  defined  in  three  dimensions. 

In  the  case  of  the  extremal  control  law  of  the  CLC  approach,  we  find  that  an  addi¬ 
tional  relationship  is  required  in  order  to  facilitate  a  trade-off  between  turn  rate  and  pitch 
rate.  A  simple  maximum  turn  rate  constraint  was  sufficient  in  the  2-dimensional  problem 
of  [1]  because  it  was  assumed  that  the  air  vehicle  maintained  a  specific  altitude  throughout 
its  flight.  When  we  relax  this  assumption  and  allow  the  vehicle  to  be  free  to  move  in  the 
full  3-dimensional  space,  we  obviously  need  to  place  a  constraint  on  the  maximum  pitch 
rate  as  well  as  the  turn  rate.  Moreover,  given  that  the  vehicle’s  three  axes  of  motion  are 
not  decoupled,  a  third  constraint  must  be  invoked  to  describe  the  coupling  between  turn 
rate  and  pitch  rate. 

Finally,  the  results  of  applying  input  commands  generated  by  the  minimum  effort 
control  technique  were  discussed  in  Section  4.5.  Through  simulated  target  engagement 
scenarios,  we  have  determined  that  this  input  generation  method  is  an  inadequate  option. 
In  general,  the  minimum  effort  control  technique  is  not  useful  for  this  class  of  problem, 
since  it  does  not  have  the  ability  to  follow  a  desired  path  between  waypoints.  The  negative 
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impact  of  this  limitation  varies  based  on  the  setup  of  the  scenario  under  consideration.  For 
cases  in  which  the  target  is  located  close  to  a  large  heading  change,  we  simply  fly  past  the 
target  before  the  turn  is  completed,  but  at  least  the  munition  is  able  to  continue  to  engage 
targets.  In  the  more  severe  case  of  a  guidance  path  which  attempts  to  fly  the  munition 
along  a  narrow  corridor  between  obstacles,  we  may  actually  fail  to  achieve  the  obstacle 
avoidance  requirement  and  fly  the  munition  into  the  side  of  a  mountain. 

5.3  Recommendations 

Given  the  successes  and  failures  of  this  research,  several  recommendations  can  be 
made  for  future  efforts  on  this  topic. 

5.3.1  CLC  Optimization  in  3  Dimensions.  As  stated  in  Section  3.5.1,  the  exten¬ 
sion  of  the  CLC  (extremal)  control  optimization  technique  is  incomplete.  In  order  to  finish 
this  extension,  a  relationship  must  be  established  between  turn  rate  and  pitch  rate.  The 
requirement  for  this  relationship  is  motivated  by  the  fact  that  motion  in  the  horizontal  and 
vertical  planes  of  the  air  vehicle  are  not  fully  decoupled.  For  example,  when  the  munition 
attempts  to  execute  a  horizontal  coordinated  turn,  the  air  vehicle  is  forced  into  a  dive 
unless  an  intentional  effort  is  made  to  counteract  this  effect.  While  it  is  known  that  this 
relationship  exists,  the  exact  (or  even  an  approximate)  form  of  the  relationship  is  unknown. 
Once  this  trade-off  between  turn  rate  and  pitch  rate  is  characterized,  the  extension  of  the 
CLC  control  optimization  technique  may  be  completed  in  a  manner  similar  to  [1]. 

(r) 

5.3.2  MPC  Implementation.  As  shown  throughout  this  research,  the  MATLABW 
implementation  of  the  MPC  optimization  technique  is  flawed,  such  that  the  simulation 
cannot  even  complete  a  flight  along  steady- level  path.  The  notion  that  the  flaw  resides 
within  this  specific  implementation  is  supported  by  the  fact  that  other  researchers  [9, 15] 
have  been  successful  in  using  the  MPC  technique.  Bearing  this  in  mind,  it  would  be 
beneficial  to  work  on  coding  a  2-dinrensional  problem  properly  and  then  expand  the  utility 
of  this  technique  to  3-dimensional  problems  via  the  method  presented  in  this  thesis. 
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5.3.3  Enhanced  Waypoint  Generation  Technique.  The  waypoint  generation  ap¬ 
proach  utilized  throughout  this  research  is  a  simple  method  designed  to  facilitate  the 
investigation  of  the  minimum  effort  path  selection  technique.  A  desired  enhancement  for 
the  waypoint  generation  function  is  to  allow  for  a  terrain-following  feature  so  that  the  ve¬ 
hicle  may  remain  close  to  the  ground  and  avoid  detection.  In  order  to  realize  this  feature, 
we  would  seek  to  fly  at  a  low  altitude  as  often  as  possible  and,  when  we  are  forced  to 
a  higher  altitude,  to  execute  climbing  maneuvers  at  the  maximum  reasonable  flight-path 
angle.  This  improvement  would  also  require  an  alteration  to  the  relative  weighting  values 
in  the  cost  computation. 

5.3.4  CLC  Assistance  to  Waypoint  Generator.  The  CLC  command  generation 
method  is  a  very  intuitive  approach  and,  once  the  extension  to  3  dimensions  is  complete, 
elements  of  the  CLC  optimizations  technique  may  be  used  to  augment  the  quality  of  the 
path  planning  process.  Specifically,  the  details  of  the  /-e-trajectories  dictate  where  the 
waypoint  can  be  placed,  relative  to  the  corner  of  the  obstacle,  in  order  to  ensure  that  the 
munition’s  actual  trajectory  does  not  impact  an  obstacle  when  the  control  inputs  drive  the 
trajectory  away  from  the  LOS  path  during  heading  changes. 

5.3.5  Remove  Altitude  Bias  from  Energy  Computation.  The  structure  of  the  lin¬ 
earized  model  used  for  the  controllability  Grammian  computation  leads  to  an  inherent  bias 
towards  flying  at  low  altitudes.  This  happens  because  the  definition  of  the  control  input 
vector  contains  an  element  for  absolute  altitude  and  a  squared  altitude  value  will  become 
involved  in  any  control  input  energy  computation.  Additionally,  the  control  input  vector 
composed  of  elements  for  speed,  altitude,  and  heading  does  not  provide  an  accurate  repre¬ 
sentation  of  the  control  energy  since  these  three  inputs  are  actually  pseudo  inputs.  A  more 
logical  choice,  would  be  to  minimize  the  control  energy  for  the  control  input  vector  (throt¬ 
tle  setting  and  3  actuator  deflection  angles)  that  is  applied  to  the  non-linear  air  vehicle 
model.  This  change  can  be  achieved  by  altering  the  structure  of  the  model’s  input  control 
vector  and  redesigning  the  controller  to  meet  stability  and  performance  requirements. 
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5.3.6  Situational  Logic  for  Target  Designation  Change  Scenario.  In  the  target 
designation  change  scenario,  we  noted  that  the  path  planning  algorithm  was  able  to  react  to 
the  updated  target  location.  However,  as  shown  in  Figure  4.12,  the  munition  is  susceptible 
to  flying  past  the  target.  For  this  research,  we  designated  this  situation  as  a  failed 
engagement  and  halted  the  simulation.  A  desired  alternative  to  this  approach  is  to  allow 
the  munition  to  either  search  for  a  new  target  or  to  attempt  a  "second  pass"  against  the 
current  target.  These  two  alternative  options  may  be  implemented  by  using  situation- 
based  logic  to  determine  the  appropriate  coarse  of  action  and,  if  necessary,  a  modified 
version  of  the  path  planning  algorithm  to  determine  the  reference  trajectory  for  a  "second 
pass"  engagement. 

5.3.7  Streamline  Simulation  Code  for  Efficiency.  Finally,  an  effort  needs  to 

(r) 

be  made  to  clean  up  the  MATLAE>  code  implementation  in  order  to  streamline  the 
path  planning  algorithm.  One  major  step  is  to  store  the  candidate  paths  in  a  branching 
tree  structure,  instead  of  a  growing  length  list,  so  that  we  are  able  to  avoid  redundant 
computations  when  generating  the  path  cost  for  the  path  selection  process.  Another 
major  focus  ares  is  to  prune  off  candidate  paths  earlier  in  the  algorithm  in  an  effort  to 
reduce  the  number  of  unrealistic  paths  which  must  be  maintained  and  evaluated  throughout 
the  simulation.  These  two  courses  of  action  could  significantly  reduce  the  computation 
complexity  of  the  path  selection  algorithm. 
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